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

import ch.trq.carrera.javapilot.akka.trackanalyzer.Track;
import ch.trq.carrera.javapilot.akka.trackanalyzer.TrackSection;
import com.zuehlke.carrera.relayapi.messages.SensorEvent;

/**
 * Created by tourn on 22.10.15.
 */
public class PositionTracker {
    private final Track track;
    private long tLastUpdate = -1;
    private UpdateCallback onUpdate;
    private SectionChangeCallback onSectionChange;
    private Track.Position pos;
    private int sectionIndex;

    public PositionTracker(Track track){
        this.track = track;
        pos = new Track.Position(track.getSections().get(0), 0);
        sectionIndex = 0;
    }

    public void update(SensorEvent e){
        if(tLastUpdate == -1){
            tLastUpdate = e.getTimeStamp();
            return;
        }
        long offset = e.getTimeStamp() - tLastUpdate;
        tLastUpdate = e.getTimeStamp();
        //calculate things

        pos.setDurationOffset(pos.getDurationOffset()+offset);
        if(sectionChanged()){
            if(onSectionChange!=null){
                onSectionChange.onUpdate(sectionIndex, pos.getSection());
            }

            sectionIndex = (sectionIndex +1) % track.getSections().size();
            TrackSection next = track.getSections().get(sectionIndex);
            pos.setSection(next);
            pos.setDurationOffset(0); //add overshoot?

        }
        if(onUpdate != null){
            onUpdate.onUpdate(sectionIndex, pos.getDurationOffset());
        }
    }

    private boolean sectionChanged(){
        return pos.getSection().getDuration() < pos.getDurationOffset();
    }

    public static abstract class UpdateCallback{
        public abstract void onUpdate(int sectionIndex, long offset);
    }

    public static abstract class SectionChangeCallback{
        public abstract void onUpdate(int sectionIndex, TrackSection section);
    }

    public void setOnSectionChange(SectionChangeCallback onSectionChange) {
        this.onSectionChange = onSectionChange;
    }

    public void setOnUpdate(UpdateCallback onUpdate) {
        this.onUpdate = onUpdate;
    }
}

@@ -229,16 +229,128 @@
 orEvent;
+%0Aimport com.zuehlke.carrera.timeseries.FloatingHistory;%0Aimport org.slf4j.Logger;%0Aimport org.slf4j.LoggerFactory;
 %0A%0A/**%0A *
@@ -630,16 +630,207 @@
 onIndex;
+%0A    private FloatingHistory gyroZ = new FloatingHistory(8);%0A    private static double TURN_THRESHOLD = 1500;%0A    private final Logger LOGGER = LoggerFactory.getLogger(PositionTracker.class);
 %0A%0A    pu
@@ -1029,16 +1029,51 @@
 ent e)%7B%0A
+        gyroZ.shift(e.getG()%5B2%5D);%0A%0A
         
@@ -1871,24 +1871,24 @@
    %7D%0A    %7D%0A%0A
-
     private 
@@ -1913,16 +1913,578 @@
 nged()%7B%0A
+        //LOGGER.info(%22Selection changed ???%22);%0A        if(pos.getSection().getDuration() %3C (2*pos.getDurationOffset()))%7B%0A            if(pos.getSection().getDirection().equals(%22GOING STRAIGHT%22) && Math.abs(gyroZ.currentMean()) %3E TURN_THRESHOLD)%7B%0A                LOGGER.info(%22going into TURN%22);%0A                return true;%0A            %7D%0A            if(pos.getSection().getDirection().equals(%22TURN%22) && Math.abs(gyroZ.currentMean()) %3C TURN_THRESHOLD)%7B%0A                LOGGER.info(%22going into GOING STRAIGHT%22);%0A                return true;%0A            %7D%0A        %7D%0A
         

add gyroz to positiontracker