<commit_before>/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package ch.unizh.ini.jaer.projects.gesture.stereo;
import ch.unizh.ini.jaer.hardware.pantilt.PanTilt;
import ch.unizh.ini.jaer.projects.gesture.virtualdrummer.BlurringFilter2DTracker.Cluster;
import com.sun.opengl.util.GLUT;
import java.awt.geom.*;
import java.util.List;
import java.util.Observable;
import java.util.Observer;
import javax.media.opengl.GL;
import javax.media.opengl.GLAutoDrawable;
import net.sf.jaer.chip.AEChip;
import net.sf.jaer.event.EventPacket;
import net.sf.jaer.eventprocessing.EventFilter2D;
import net.sf.jaer.eventprocessing.FilterChain;
import net.sf.jaer.graphics.FrameAnnotater;
import net.sf.jaer.hardwareinterface.*;
import net.sf.jaer.util.filter.LowpassFilter2d;
/**
 *  Simple demonstration of stereo disparity using pantilt servo to wave hand to show output of stereo blob tracking.
 * @author <NAME>, <NAME>
 *
 * This is part of jAER
<a href="http://jaer.wiki.sourceforge.net">jaer.wiki.sourceforge.net</a>,
licensed under the LGPL (<a href="http://en.wikipedia.org/wiki/GNU_Lesser_General_Public_License">http://en.wikipedia.org/wiki/GNU_Lesser_General_Public_License</a>.
 */
public class StereoWavingHandRobotDemo extends EventFilter2D implements FrameAnnotater,Observer{
    private float servoLimit = getPrefs().getFloat("StereoWavingHandRobotDemo.servoLimit",0.25f);
    private float disparityScaling = getPrefs().getFloat("StereoWavingHandRobotDemo.disparityScaling",0.05f);
    private float tauArmMs = getPrefs().getFloat("StereoWavingHandRobotDemo.tauArmMs",20);
    private FilterChain chain = null;
    BlurringFilterStereoTracker tracker = null;
    PanTilt panTilt = null;
    LowpassFilter2d filter = new LowpassFilter2d(tauArmMs);
    private final int SERVO_RETRY_INTERVAL = 300;
    private boolean servosEnabled = getPrefs().getBoolean("StereoWavingHandRobotDemo.servosEnabled",true);

    public StereoWavingHandRobotDemo (AEChip chip){
        super(chip);
        setPropertyTooltip("servoLimit","limits servos to this value around 0.5f");
        setPropertyTooltip("disparityScaling","disparity values are scaled by this factor and divided by disparity limit of BlurringFilterStereoTracker to arrive at servo position");
        setPropertyTooltip("servosEnabled","enable servo outputs");
        setPropertyTooltip("tauArmMs","lowpass filter time constant for pan-tilt outputs in ms");
        tracker = new BlurringFilterStereoTracker(chip);
        tracker.addObserver(this);
        chain = new FilterChain(chip);
        chain.add(tracker);
        setEnclosedFilterChain(chain);
    }

    @Override
    synchronized public EventPacket<?> filterPacket (EventPacket<?> in){
        in = tracker.filterPacket(in);
//        moveArm();  // rely on updates from tracker at updateIntervalMs to move arm
        return in;
    }

    @Override
    synchronized public void resetFilter (){
        setPanTilt(.5f,.5f);
        filter.setInternalValue2d(.5f,.5f);
    }

    private float clipServo (float f){
        if ( f < 0.5f - servoLimit / 2 ){
            f = .5f - servoLimit / 2;
        } else if ( f > .5f + servoLimit / 2 ){
            f = .5f + servoLimit / 2;
        }
        return f;
    }
    private int servoRetryCounter = 0;
    private boolean printedServoWarning = false;

    private void setPanTilt (float pan,float tilt){
        if ( !servosEnabled ){
            if ( !printedServoWarning ){
                printedServoWarning = true;
                log.warning("Set servosEnabled true to enable servo output");
            }
            return;
        }
        pan = clipServo(pan);
        tilt = clipServo(tilt);
        if ( panTilt != null && servoRetryCounter < 0 ){ // don't keep retrying every time
            try{
                panTilt.setPanTiltValues(pan,tilt);
            } catch ( HardwareInterfaceException e ){
                log.warning(e.toString());
                servoRetryCounter = SERVO_RETRY_INTERVAL;
            }
        }
        servoRetryCounter--;
    }

    @Override
    public void initFilter (){
        panTilt = new PanTilt();
        resetFilter();
    }
    private GLUT glut = new GLUT();

    public void annotate (GLAutoDrawable drawable){
        GL gl = drawable.getGL();
        gl.glPushMatrix();
        gl.glRasterPos3f(1,chip.getSizeY() - 1,0);
        Point2D.Float p = filter.getValue2d();
        glut.glutBitmapString(GLUT.BITMAP_HELVETICA_18,String.format("pan=%.2f tilt=%.2f",p.x,p.y));
//       gl.glColor3f(0,0,1);
//        gl.glLineWidth(5);
//        final int W=10;
//        float x=p.x*chip.getSizeX();
//        float y=p.y*chip.getSizeY();
//        gl.glRectf(x-W,y-W,x+W,y+W);

        gl.glPopMatrix();
    }

    @Override
    public synchronized void setFilterEnabled (boolean yes){
        super.setFilterEnabled(yes);
        if ( yes ){
            if ( panTilt == null ){
                panTilt = new PanTilt();
            }
        } else{
            if ( panTilt != null ){
                panTilt.close();
            }
        }
    }

    private Cluster findCluster (){
        List<Cluster> clusters = tracker.getClusters();
        Cluster sc = null;
        for ( Cluster c:clusters ){

            if ( c.isVisible() ){
                sc = c;
                break;
            }
        }
        return sc;
    }

    private void moveArm (int timestamp){
        Cluster sc = findCluster();
        if ( sc != null ){
            Point2D.Float p = sc.getLocation();
            float tilt = (float)( -1 * ( ( tracker.getDisparity() ) / ( chip.getSizeX() * .7f ) ) + 1 ) / 2 * disparityScaling;
            float pan = (float)p.getX() / chip.getSizeX();
            Point2D.Float pt = filter.filter2d(pan,tilt,timestamp);<commit_msg>new movement plan was implemented<commit_after>/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package ch.unizh.ini.jaer.projects.gesture.stereo;
import ch.unizh.ini.jaer.hardware.pantilt.PanTilt;
import ch.unizh.ini.jaer.projects.gesture.virtualdrummer.BlurringFilter2DTracker.Cluster;
import com.sun.opengl.util.GLUT;
import java.awt.geom.*;
import java.util.List;
import java.util.Observable;
import java.util.Observer;
import javax.media.opengl.GL;
import javax.media.opengl.GLAutoDrawable;
import net.sf.jaer.chip.AEChip;
import net.sf.jaer.event.EventPacket;
import net.sf.jaer.eventprocessing.EventFilter2D;
import net.sf.jaer.eventprocessing.FilterChain;
import net.sf.jaer.graphics.FrameAnnotater;
import net.sf.jaer.hardwareinterface.*;
import net.sf.jaer.util.filter.LowpassFilter2d;
/**
 *  Simple demonstration of stereo disparity using pantilt servo to wave hand to show output of stereo blob tracking.
 * @author <NAME>, <NAME>
 *
 * This is part of jAER
<a href="http://jaer.wiki.sourceforge.net">jaer.wiki.sourceforge.net</a>,
licensed under the LGPL (<a href="http://en.wikipedia.org/wiki/GNU_Lesser_General_Public_License">http://en.wikipedia.org/wiki/GNU_Lesser_General_Public_License</a>.
 */
public class StereoWavingHandRobotDemo extends EventFilter2D implements FrameAnnotater,Observer{
    private float servoLimit = getPrefs().getFloat("StereoWavingHandRobotDemo.servoLimit",0.25f);
    private float disparityScaling = getPrefs().getFloat("StereoWavingHandRobotDemo.disparityScaling",0.05f);
    private float tauArmMs = getPrefs().getFloat("StereoWavingHandRobotDemo.tauArmMs",20);
    private FilterChain chain = null;
    BlurringFilterStereoTracker tracker = null;
    PanTilt panTilt = null;
    LowpassFilter2d filter = new LowpassFilter2d(tauArmMs);
    private final int SERVO_RETRY_INTERVAL = 300;
    private int sizeX = 0, sizeY = 0;
    private boolean servosEnabled=getPrefs().getBoolean("StereoWavingHandRobotDemo.servosEnabled",true);

    public StereoWavingHandRobotDemo (AEChip chip){
        super(chip);
        setPropertyTooltip("servoLimit","limits servos to this value around 0.5f");
        setPropertyTooltip("disparityScaling","disparity values are scaled by this factor and divided by disparity limit of BlurringFilterStereoTracker to arrive at servo position");
        setPropertyTooltip("servosEnabled","enable servo outputs");
        setPropertyTooltip("tauArmMs","lowpass filter time constant for pan-tilt outputs in ms");
        tracker = new BlurringFilterStereoTracker(chip);
        tracker.addObserver(this);
        chain = new FilterChain(chip);
        chain.add(tracker);
        setEnclosedFilterChain(chain);
    }

    @Override
    synchronized public EventPacket<?> filterPacket (EventPacket<?> in){
        if(sizeX == 0){
            sizeX = chip.getSizeX();
            sizeY = chip.getSizeY();
        }
        in = tracker.filterPacket(in);
//        moveArm();  // rely on updates from tracker at updateIntervalMs to move arm
        return in;
    }

    @Override
    synchronized public void resetFilter (){
        setPanTilt(.5f,.5f);
        filter.setInternalValue2d(.5f,.5f);
    }

    private float clipServo (float f){
        if ( f < 0.5f - servoLimit ){
            f = .5f - servoLimit;
        } else if ( f > .5f + servoLimit ){
            f = .5f + servoLimit;
        }
        return f;
    }
    private int servoRetryCounter = 0;
    private boolean printedServoWarning = false;

    private void setPanTilt (float pan,float tilt){
        if ( !servosEnabled ){
            if ( !printedServoWarning ){
                printedServoWarning = true;
                log.warning("Set servosEnabled true to enable servo output");
            }
            return;
        }
        pan = clipServo(pan);
        tilt = clipServo(tilt);
        if ( panTilt != null && servoRetryCounter < 0 ){ // don't keep retrying every time
            try{
                panTilt.setPanTiltValues(pan,tilt);
            } catch ( HardwareInterfaceException e ){
                log.warning(e.toString());
                servoRetryCounter = SERVO_RETRY_INTERVAL;
            }
        }
        servoRetryCounter--;
    }

    @Override
    public void initFilter (){
        panTilt = new PanTilt();
        resetFilter();
    }
    private GLUT glut = new GLUT();

    public void annotate (GLAutoDrawable drawable){
        GL gl = drawable.getGL();
        gl.glPushMatrix();
        gl.glRasterPos3f(1,chip.getSizeY() - 1,0);
        Point2D.Float p = filter.getValue2d();
        glut.glutBitmapString(GLUT.BITMAP_HELVETICA_18,String.format("pan=%.2f tilt=%.2f",p.x,p.y));
//       gl.glColor3f(0,0,1);
//        gl.glLineWidth(5);
//        final int W=10;
//        float x=p.x*chip.getSizeX();
//        float y=p.y*chip.getSizeY();
//        gl.glRectf(x-W,y-W,x+W,y+W);

        gl.glPopMatrix();
    }

    @Override
    public synchronized void setFilterEnabled (boolean yes){
        super.setFilterEnabled(yes);
        if ( yes ){
            if ( panTilt == null ){
                panTilt = new PanTilt();
            }
        } else{
            if ( panTilt != null ){
                panTilt.close();
            }
        }
    }

    private Cluster findCluster (){
        List<Cluster> clusters = tracker.getClusters();
        Cluster sc = null;
        for ( Cluster c:clusters ){

            if ( c.isVisible() ){
                sc = c;
                break;
            }
        }
        return sc;
    }

    private void moveArm (int timestamp){
        Cluster sc = findCluster();
        if ( sc != null ){
            Point2D.Float p = sc.getLocation();
            float disparity = (float)(((tracker.getDisparity() ) / (chip.getSizeX()*.7f))+1)/2 * disparityScaling;
            float tilt = (float)p.getY() / sizeY;
            float pan = (float)p.getX() / sizeX;

            tilt = 0.5f + 2.0f*servoLimit * ((1.0f - 0.8f*disparity)*tilt - 0.5f);
            pan = 0.5f + 2.0f*servoLimit * ((1.0f - 0.8f*disparity)*pan - 0.5f);

            Point2D.Float pt = filter.filter2d(pan,tilt,timestamp);
{'fraction_non_alphanumeric': 0.11550935550935551, 'fraction_numerical': 0.011309771309771309, 'mean_word_length': 2.539140670982931, 'pattern_counts': {'":': 0, '<': 25, '<?xml version=': 0, '>': 23, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 1, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '1795682', 'n_tokens_mistral': 3881, 'n_tokens_neox': 3712, 'n_words': 809}