package ch.trq.carrera.javapilot.akka.trackanalyzer;

import ch.trq.carrera.javapilot.math.TrackPhysicsModel;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

import java.util.List;
import java.util.ArrayList;
import java.util.Iterator;
import java.util.function.Consumer;

/**
 * Created by Frank on 22.09.2015.
 */
public class TrackAnalyzer {
    private List<TrackSection> trackSections = new ArrayList<>();
    private List<TrackVelocity> trackVelocities = new ArrayList<>();
    private TrackSection tempTrackSection;
    private final Logger LOGGER = LoggerFactory.getLogger(TrackAnalyzer.class);

    private TrackRecognitionCallback onTrackRecognized;

    private int faultyGoingStraightTime = 200;
    private int faultyTurnTime = 250;
    private int ignoredTrackSections = 3;
    private int trackVelocitiesPerRound = 4;
    private int minTrackSectionsPerRound = 4;


    public TrackAnalyzer(){

    }

    /**
     *
     *
     * @param velocity The velocity from the velocity-sensor-message
     * @param timeStamp The received time-stamp from the velocity-sensor-message
     */
    public void addTrackVelocity(double velocity, long timeStamp){
        trackVelocities.add(new TrackVelocity(velocity, timeStamp));
    }


    public void addTrackSection(State state, long timeStamp){
        if(tempTrackSection != null){
            tempTrackSection.setDuration(timeStamp - tempTrackSection.getTimeStamp());
            switch(tempTrackSection.getDirection()){
                case STRAIGHT:
                    if(tempTrackSection.getDuration()<faultyGoingStraightTime){
                        if(getLastTrackSection()!=null){
                            getLastTrackSection().addDuration(tempTrackSection.getDuration());
                        }
                    }else if(getLastTrackSection()!=null && getLastTrackSection().getDirection()==State.STRAIGHT){
                        getLastTrackSection().addDuration(tempTrackSection.getDuration());
                    }else{
                        trackSections.add(tempTrackSection);
                    }
                    break;
                case LEFT:
                case RIGHT:
                    if(tempTrackSection.getDuration()<faultyTurnTime){
                        if(getLastTrackSection()!=null){
                            getLastTrackSection().addDuration(tempTrackSection.getDuration());
                        }
                    }else if(getLastTrackSection()!=null && (getLastTrackSection().getDirection()==State.LEFT || getLastTrackSection().getDirection()==State.RIGHT)){
                        getLastTrackSection().addDuration(tempTrackSection.getDuration());
                    }else{
                        trackSections.add(tempTrackSection);
                    }
                    break;
            }
        }
        tempTrackSection = new TrackSection(state,timeStamp);
        tryToFindRoundCycle();
        //printTrackSections();
    }

    private void tryToFindRoundCycle(){
        if(foundTrackCycle()){
            if(onTrackRecognized != null){
                Track track = null;
                onTrackRecognized.onTrackRecognized(track);
            }
            LOGGER.info("FOUND TRACK CYCLE: " + (trackSections.size()-ignoredTrackSections)/2 + " sections");
            printTrackSections();
        }
    }

    private boolean foundTrackCycle(){
        if(trackSections.size()>=ignoredTrackSections+2*minTrackSectionsPerRound && (trackSections.size()-ignoredTrackSections)%2==0){
            int halfTrackSectionCount = (trackSections.size()-ignoredTrackSections)/2;
            for(int i = ignoredTrackSections; i < ignoredTrackSections+halfTrackSectionCount; i++){
                if(trackSections.get(i).getDirection()!=trackSections.get(i+halfTrackSectionCount).getDirection()){
                    return false;
                }
            }
            return true;
        }else{
            return false;
        }
    }

    private TrackSection getLastTrackSection(){
        if(trackSections.size()==0){
            return null;
        }
        return trackSections.get(trackSections.size()-1);
    }


    public static abstract class TrackRecognitionCallback{
        public abstract void onTrackRecognized(Track track);
    }

    public void setOnTrackRecognized(TrackRecognitionCallback onTrackRecognized){
        this.onTrackRecognized = onTrackRecognized;
    }


    private void printTrackSections(){
        String s="";
        for(TrackSection trackSection : trackSections){
            switch(trackSection.getDirection()){
                case STRAIGHT:
                    s+="S";
                    break;
                case LEFT:
                    s+="L";
                    break;
                case RIGHT:
                    s+="R";
                    break;
            }
        }
        LOGGER.info("\n"+s);
    }
}

@@ -3114,20 +3114,28 @@
 track = 
-null
+buildTrack()
 ;%0A      
@@ -4166,32 +4166,1399 @@
 ze()-1);%0A    %7D%0A%0A
+    private Track buildTrack()%7B%0A        removeIgnoredTrackSections();%0A        removeIgnoredTrackVelocities();%0A        Track track = new Track();%0A        track.getSections().addAll(getAveragesTrackSectionData());%0A        return track;%0A    %7D%0A%0A    private void removeIgnoredTrackSections()%7B%0A        for(int i = 0; i%3CignoredTrackSections; i++)%7B%0A            trackSections.remove(0);%0A        %7D%0A    %7D%0A%0A    private void removeIgnoredTrackVelocities()%7B%0A        while(trackVelocities.get(0).getTimeStamp()%3CtrackSections.get(0).getTimeStamp())%7B%0A            trackVelocities.remove(0);%0A        %7D%0A    %7D%0A%0A    private List%3CTrackSection%3E getAveragesTrackSectionData()%7B%0A        List%3CTrackSection%3E list = new ArrayList%3C%3E();%0A        int trackSectionsPerRound = trackSections.size()/2;%0A        for(int i = 0; i %3C trackSectionsPerRound; i++)%7B%0A            long timeStamp;%0A            if(i==0)%7B%0A                timeStamp = 0;%0A            %7Delse%7B%0A                timeStamp = list.get(i-1).getTimeStamp()+list.get(i-1).getDuration();%0A            %7D%0A            State state = trackSections.get(i).getDirection();%0A            TrackSection trackSection = new TrackSection(state,timeStamp);%0A            trackSection.setDuration((trackSections.get(i).getDuration()+trackSections.get(i+trackSectionsPerRound).getDuration())/2);%0A            list.add(trackSection);%0A        %7D%0A        return list;%0A    %7D%0A
 %0A    public stat
@@ -5803,24 +5803,23 @@
 gnized;%0A
+
     %7D%0A%0A
-%0A
     priv

add trackbuilder function, which calculates the track