package main

import (
    "fmt"
    "os/exec"
    "time"

    "gobot.io/x/gobot"
    "gobot.io/x/gobot/platforms/keyboard"
    "gobot.io/x/gobot/platforms/dji/tello"
)

func main() {
    drone := tello.NewDriver("8890")
    keys := keyboard.NewDriver()

    work := func() {
        /**
        ドローンカメラアクセス
        */
        mplayer := exec.Command("mplayer", "-fps", "25", "-")
        mplayerIn, _ := mplayer.StdinPipe()
        if err := mplayer.Start(); err != nil {
            fmt.Println(err)
            return
        }

        drone.On(tello.ConnectedEvent, func(data interface{}) {
            fmt.Println("Connected")
            drone.StartVideo()
            drone.SetVideoEncoderRate(4)
            gobot.Every(100*time.Millisecond, func() {
                drone.StartVideo()
            })
        })

        drone.On(tello.VideoFrameEvent, func(data interface{}) {
            pkt := data.([]byte)
            if _, err := mplayerIn.Write(pkt); err != nil {
                fmt.Println(err)
            }
        })

        /**
        ドローン制御
        */
        keys.On(keyboard.Key, func(data interface{}) {
            key := data.(keyboard.KeyEvent)

            if key.Key == keyboard.C {
                fmt.Println("Command Test")
            } else if key.Key == keyboard.T{
                fmt.Println("Take Off!")
                drone.TakeOff() //離陸
            } else if key.Key == keyboard.L{
                fmt.Println("Land")
                drone.Land() //着陸
            } else if key.Key == keyboard.ArrowUp{
                fmt.Println("go foward")
		drone.Forward(10) //前進
	    } else if key.Key == keyboard.ArrowDown{
                fmt.Println("go backword")
		drone.Backward(10)
	    } else if key.Key == keyboard.ArrowLeft{
                fmt.Println("drift Left")
		drone.Left(10)
	    } else if key.Key == keyboard.ArrowRight{
                fmt.Println("drift Right")
		drone.Right(10)
            } else if key.Key == keyboard.W{
                fmt.Println("go Up")
                drone.Up(10)
            } else if key.Key == keyboard.S{
                fmt.Println("go down")
                drone.Down(10) 
	    } else if key.Key == keyboard.A{
                fmt.Println("turn Left")
                drone.CounterClockwise(10)
            } else if key.Key == keyboard.D{
                fmt.Println("turn Right")
                drone.Clockwise(10)
	    } else if key.Key == keyboard.Z{
                fmt.Println("parameter flush")
                drone.Forward(0)
		drone.Backward(0)
		drone.Left(0)
                drone.Right(0)
		drone.Up(0)
		drone.Down(0)
		drone.CounterClockwise(0)
                drone.Clockwise(0)
            } else {
                fmt.Println("keyboard event!", key, key.Char)
            }
        })
    }

    robot := gobot.NewRobot("tello",
        []gobot.Connection{},
        []gobot.Device{drone, keys}, //droneとkeysの二つのアダプタ
        work,
    )

    robot.Start()
}