package main

import (
	"fmt"
	"log"

	"douglasthrift.net/dtrobots/gamepad"

	"github.com/ev3go/ev3dev"
)

// drive modes
const (
	tank = iota
	joystick
)

const driveModeCount = 2

type driveMode int

func (d driveMode) String() string {
	switch d {
	case tank:
		return "tank"
	case joystick:
		return "joystick"
	default:
		return "unknown"
	}
}

func (d *driveMode) Set(value string) error {
	switch value {
	case "tank":
		*d = tank
	case "joystick":
		*d = joystick
	default:
		return fmt.Errorf("unknown drive mode: %v", value)
	}
	return nil
}

func control(mode driveMode, g *gamepad.Gamepad, leftMotor, rightMotor *ev3dev.TachoMotor) error {
	log.Printf("control loop started (drive mode: %v)", mode)

	polarity, err := leftMotor.Polarity()
	if err != nil {
		return err
	}
	if polarity != ev3dev.Inversed {
		leftMotor.SetPolarity(ev3dev.Inversed)
	}
	if err = leftMotor.Err(); err != nil {
		return err
	}
	polarity, err = rightMotor.Polarity()
	if err != nil {
		return err
	}
	if polarity != ev3dev.Inversed {
		rightMotor.SetPolarity(ev3dev.Inversed)
	}
	if err = rightMotor.Err(); err != nil {
		return err
	}

	for {
		select {
		case e := <-g.LeftAnalog:
			switch mode {
			case tank:
				if err := tankAnalogControl(e, leftMotor); err != nil {
					return err
				}
			}
		case e := <-g.RightAnalog:
			switch mode {
			case tank:
				if err := tankAnalogControl(e, rightMotor); err != nil {
					return err
				}
			}
		case e := <-g.DPad:
			log.Println("dpad", e)
		case e := <-g.A:
			log.Println("a", e)
		case e := <-g.B:
			log.Println("b", e)
		case e := <-g.X:
			log.Println("x", e)
		case e := <-g.Y:
			log.Println("y", e)
		case e := <-g.Left:
			log.Println("left", e)
		case e := <-g.Right:
			log.Println("right", e)
		case e := <-g.Back:
			log.Println("back", e)
		case e := <-g.Start:
			log.Println("start", e)
		case e := <-g.Logitech:
			if e.IsButtonPress() {
				mode++
				mode %= driveModeCount
				log.Printf("switched drive mode: %v", mode)
			}
		}
	}
}

func tankAnalogControl(e gamepad.Event, motor *ev3dev.TachoMotor) error {
	if e.IsAxisY() {
		speed := int(float64(motor.MaxSpeed()) * e.Fraction())
		currentSpeed, err := motor.Speed()
		if err != nil {
			return err
		}
		if speed != currentSpeed {
			motor.SetSpeedSetpoint(speed)
			if speed == 0 {
				motor.Command("stop")
			} else {
				motor.Command("run-forever")
			}
			if err = motor.Err(); err != nil {
				return err
			}
		}
	}
	return nil
}