<commit_before>package main

import (
	"flag"
	"fmt"
	"net"
	"net/http"
	_ "net/http/pprof"
	"os"
	"os/signal"
	"sync"
	"syscall"
	"time"

	pb "github.com/viru/berrybot/proto"

	log "github.com/Sirupsen/logrus"
	"github.com/kidoman/embd"
	_ "github.com/kidoman/embd/host/rpi" // RaspberryPI driver
	"google.golang.org/grpc"
)

// Server is used to implement steering.DriverServer.
type server struct {
	front, rear *echo
	driver      *driver
}

// Proximity sensor.
type echo struct {
	name    string
	echo    embd.DigitalPin
	trig    embd.DigitalPin
	waitc   chan struct{}
	dist    int64
	last    time.Time
	enabled bool
	send    chan bool
}

func newEcho(name string, trigPin, echoPin int) (*echo, error) {
	var e echo
	e.name = name
	e.waitc = make(chan struct{})
	e.send = make(chan bool)
	var err error
	e.trig, err = embd.NewDigitalPin(trigPin)
	if err != nil {
		return nil, fmt.Errorf("can't init trigger pin: %v", err)
	}
	e.echo, err = embd.NewDigitalPin(echoPin)
	if err != nil {
		return nil, fmt.Errorf("can't init echo pin: %v", err)
	}

	// Set direction.
	if err := e.trig.SetDirection(embd.Out); err != nil {
		return nil, fmt.Errorf("can't set trigger direction: %v", err)
	}
	if err := e.echo.SetDirection(embd.In); err != nil {
		return nil, fmt.Errorf("can't set echo direction: %v", err)
	}
	return &e, nil
}

// Try to TimePulse proximity sensor to calculate distance.
func (e *echo) measure() error {
	if err := e.trig.Write(embd.High); err != nil {
		return fmt.Errorf("can't set trigger to high: %v", err)
	}
	time.Sleep(time.Microsecond * 10)
	if err := e.trig.Write(embd.Low); err != nil {
		return fmt.Errorf("can't set trigger to low: %v", err)
	}
	dur, err := e.echo.TimePulse(embd.High)
	if err != nil {
		return fmt.Errorf("can't time pulse: %v", err)
	}
	log.Infof("%s: distance: %dcm", e.name, dur.Nanoseconds()/1000*34/1000/2)
	e.dist = dur.Nanoseconds() / 1000 * 34 / 1000 / 2
	e.send <- true
	return nil
}

const (
	defaultFastDur = time.Millisecond * 250
	defaultSlowDur = time.Second
)

// Goroutine measuring distance in an infinite loop. If distancer is enabled (bot is driving)
// then measuring on fast timer, otherwise only once per second to save CPU cycles.
func (e *echo) runDistancer() {
	if err := e.trig.Write(embd.Low); err != nil {
		log.Warnf("can't set trigger to low: %v", err)
	}
	time.Sleep(time.Second * 1) // Settle time needed after initial activation.
	fast := time.NewTicker(defaultFastDur)
	defer fast.Stop()
	slow := time.NewTicker(defaultSlowDur)
	defer slow.Stop()
	for {
		select {
		case <-e.waitc:
			return
		case <-slow.C:
			if err := e.measure(); err != nil {
				log.Warn(err)
			}
		case <-fast.C:
			if e.enabled {
				if err := e.measure(); err != nil {
					log.Warn(err)
				}
			}
		}
	}
}

func (e *echo) close() {
	close(e.waitc)
	e.echo.Close()
	e.trig.Close()
}

type driver struct {
	left, right *engine
	mu          sync.Mutex
	moving      bool
	last        time.Time
}

func (d *driver) safetyStop() {
	ticker := time.NewTicker(time.Second)
	for range ticker.C {
		d.mu.Lock()
		if d.moving && d.last.Add(time.Second).Before(time.Now()) {
			d.mu.Unlock()
			d.stop()
			log.Warn("Emergency stop!")
			continue
		}
		d.mu.Unlock()
	}
}

func (d *driver) setMoving(moving bool) {
	d.mu.Lock()
	d.last = time.Now()
	d.moving = moving
	d.mu.Unlock()
}

func (d *driver) stop() {
	d.left.pwr = 0
	d.right.pwr = 0
	d.setMoving(false)
}

func (d *driver) forward(pwr int32) {
	d.left.pwr = pwr
	d.left.fwdPin.Write(embd.High)
	d.right.pwr = pwr
	d.right.fwdPin.Write(embd.High)
	d.setMoving(true)
}

func (d *driver) backward(pwr int32) {
	d.left.pwr = pwr
	d.left.fwdPin.Write(embd.Low)
	d.right.pwr = pwr
	d.right.fwdPin.Write(embd.Low)
	d.setMoving(true)
}

func (d *driver) sharpRight(pwr int32) {
	d.left.pwr = pwr
	d.left.fwdPin.Write(embd.High)
	d.right.pwr = pwr
	d.right.fwdPin.Write(embd.Low)
	d.setMoving(true)
}

func (d *driver) sharpLeft(pwr int32) {
	d.left.pwr = pwr
	d.left.fwdPin.Write(embd.Low)
	d.right.pwr = pwr
	d.right.fwdPin.Write(embd.High)
	d.setMoving(true)
}

func (d *driver) fwdRight() {
	d.left.pwr = 100
	d.left.fwdPin.Write(embd.High)
	d.right.pwr = 50
	d.right.fwdPin.Write(embd.High)
	d.setMoving(true)
}

func (d *driver) fwdLeft() {
	d.left.pwr = 50
	d.left.fwdPin.Write(embd.High)
	d.right.pwr = 100
	d.right.fwdPin.Write(embd.High)
	d.setMoving(true)
}

func (d *driver) backRight() {
	d.left.pwr = 100
	d.left.fwdPin.Write(embd.Low)
	d.right.pwr = 50
	d.right.fwdPin.Write(embd.Low)
	d.setMoving(true)
}

func (d *driver) backLeft() {
	d.left.pwr = 50
	d.left.fwdPin.Write(embd.Low)
	d.right.pwr = 100
	d.right.fwdPin.Write(embd.Low)
	d.setMoving(true)
}

func (s *server) drive(dir *pb.Direction) {
	switch {
	case dir.Dy > 15 && dir.Dx > -15 && dir.Dx < 15:
		s.front.enabled = true
		s.driver.forward(dir.Dy)
	case dir.Dy < -15 && dir.Dx > -15 && dir.Dx < 15:
		s.rear.enabled = true
		s.driver.backward(-dir.Dy)
	case dir.Dx > 15 && dir.Dy > -15 && dir.Dy < 15:
		s.driver.sharpRight(dir.Dx)
	case dir.Dx < -15 && dir.Dy > -15 && dir.Dy < 15:<commit_msg>Fix going forward/backward and turning at the same time<commit_after>package main

import (
	"flag"
	"fmt"
	"net"
	"net/http"
	_ "net/http/pprof"
	"os"
	"os/signal"
	"sync"
	"syscall"
	"time"

	pb "github.com/viru/berrybot/proto"

	log "github.com/Sirupsen/logrus"
	"github.com/kidoman/embd"
	_ "github.com/kidoman/embd/host/rpi" // RaspberryPI driver
	"google.golang.org/grpc"
)

// Server is used to implement steering.DriverServer.
type server struct {
	front, rear *echo
	driver      *driver
}

// Proximity sensor.
type echo struct {
	name    string
	echo    embd.DigitalPin
	trig    embd.DigitalPin
	waitc   chan struct{}
	dist    int64
	last    time.Time
	enabled bool
	send    chan bool
}

func newEcho(name string, trigPin, echoPin int) (*echo, error) {
	var e echo
	e.name = name
	e.waitc = make(chan struct{})
	e.send = make(chan bool)
	var err error
	e.trig, err = embd.NewDigitalPin(trigPin)
	if err != nil {
		return nil, fmt.Errorf("can't init trigger pin: %v", err)
	}
	e.echo, err = embd.NewDigitalPin(echoPin)
	if err != nil {
		return nil, fmt.Errorf("can't init echo pin: %v", err)
	}

	// Set direction.
	if err := e.trig.SetDirection(embd.Out); err != nil {
		return nil, fmt.Errorf("can't set trigger direction: %v", err)
	}
	if err := e.echo.SetDirection(embd.In); err != nil {
		return nil, fmt.Errorf("can't set echo direction: %v", err)
	}
	return &e, nil
}

// Try to TimePulse proximity sensor to calculate distance.
func (e *echo) measure() error {
	if err := e.trig.Write(embd.High); err != nil {
		return fmt.Errorf("can't set trigger to high: %v", err)
	}
	time.Sleep(time.Microsecond * 10)
	if err := e.trig.Write(embd.Low); err != nil {
		return fmt.Errorf("can't set trigger to low: %v", err)
	}
	dur, err := e.echo.TimePulse(embd.High)
	if err != nil {
		return fmt.Errorf("can't time pulse: %v", err)
	}
	log.Infof("%s: distance: %dcm", e.name, dur.Nanoseconds()/1000*34/1000/2)
	e.dist = dur.Nanoseconds() / 1000 * 34 / 1000 / 2
	e.send <- true
	return nil
}

const (
	defaultFastDur = time.Millisecond * 250
	defaultSlowDur = time.Second
)

// Goroutine measuring distance in an infinite loop. If distancer is enabled (bot is driving)
// then measuring on fast timer, otherwise only once per second to save CPU cycles.
func (e *echo) runDistancer() {
	if err := e.trig.Write(embd.Low); err != nil {
		log.Warnf("can't set trigger to low: %v", err)
	}
	time.Sleep(time.Second * 1) // Settle time needed after initial activation.
	fast := time.NewTicker(defaultFastDur)
	defer fast.Stop()
	slow := time.NewTicker(defaultSlowDur)
	defer slow.Stop()
	for {
		select {
		case <-e.waitc:
			return
		case <-slow.C:
			if err := e.measure(); err != nil {
				log.Warn(err)
			}
		case <-fast.C:
			if e.enabled {
				if err := e.measure(); err != nil {
					log.Warn(err)
				}
			}
		}
	}
}

func (e *echo) close() {
	close(e.waitc)
	e.echo.Close()
	e.trig.Close()
}

type driver struct {
	left, right *engine
	mu          sync.Mutex
	moving      bool
	last        time.Time
}

func (d *driver) safetyStop() {
	ticker := time.NewTicker(time.Second)
	for range ticker.C {
		d.mu.Lock()
		if d.moving && d.last.Add(time.Second).Before(time.Now()) {
			d.mu.Unlock()
			d.stop()
			log.Warn("Emergency stop!")
			continue
		}
		d.mu.Unlock()
	}
}

func (d *driver) setMoving(moving bool) {
	d.mu.Lock()
	d.last = time.Now()
	d.moving = moving
	d.mu.Unlock()
}

func (d *driver) stop() {
	d.left.pwr = 0
	d.right.pwr = 0
	d.setMoving(false)
}

func (d *driver) forward(pwr int32) {
	d.left.pwr = pwr
	d.left.fwdPin.Write(embd.High)
	d.right.pwr = pwr
	d.right.fwdPin.Write(embd.High)
	d.setMoving(true)
}

func (d *driver) backward(pwr int32) {
	d.left.pwr = pwr
	d.left.fwdPin.Write(embd.Low)
	d.right.pwr = pwr
	d.right.fwdPin.Write(embd.Low)
	d.setMoving(true)
}

func (d *driver) sharpRight(pwr int32) {
	d.left.pwr = pwr
	d.left.fwdPin.Write(embd.High)
	d.right.pwr = pwr
	d.right.fwdPin.Write(embd.Low)
	d.setMoving(true)
}

func (d *driver) sharpLeft(pwr int32) {
	d.left.pwr = pwr
	d.left.fwdPin.Write(embd.Low)
	d.right.pwr = pwr
	d.right.fwdPin.Write(embd.High)
	d.setMoving(true)
}

func (d *driver) fwdRight() {
	d.left.pwr = 100
	d.left.fwdPin.Write(embd.High)
	d.right.pwr = 0
	d.right.fwdPin.Write(embd.High)
	d.setMoving(true)
}

func (d *driver) fwdLeft() {
	d.left.pwr = 0
	d.left.fwdPin.Write(embd.High)
	d.right.pwr = 100
	d.right.fwdPin.Write(embd.High)
	d.setMoving(true)
}

func (d *driver) backRight() {
	d.left.pwr = 100
	d.left.fwdPin.Write(embd.Low)
	d.right.pwr = 0
	d.right.fwdPin.Write(embd.Low)
	d.setMoving(true)
}

func (d *driver) backLeft() {
	d.left.pwr = 0
	d.left.fwdPin.Write(embd.Low)
	d.right.pwr = 100
	d.right.fwdPin.Write(embd.Low)
	d.setMoving(true)
}

func (s *server) drive(dir *pb.Direction) {
	switch {
	case dir.Dy > 15 && dir.Dx > -15 && dir.Dx < 15:
		s.front.enabled = true
		s.driver.forward(dir.Dy)
	case dir.Dy < -15 && dir.Dx > -15 && dir.Dx < 15:
		s.rear.enabled = true
		s.driver.backward(-dir.Dy)
	case dir.Dx > 15 && dir.Dy > -15 && dir.Dy < 15:
		s.driver.sharpRight(dir.Dx)
	case dir.Dx < -15 && dir.Dy > -15 && dir.Dy < 15:
{'fraction_non_alphanumeric': 0.16307119042949597, 'fraction_numerical': 0.016081584624436165, 'mean_word_length': 3.152687296416938, 'pattern_counts': {'":': 0, '<': 23, '<?xml version=': 0, '>': 15, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '2798294', 'n_tokens_mistral': 4684, 'n_tokens_neox': 4317, 'n_words': 983}