package main

import (
"net/http"
"time"

"github.com/lhemala/dt/api"
"github.com/lhemala/dt/api/command_handler"
"github.com/lhemala/dt/application"
"github.com/lhemala/dt/devices/arduino"
"github.com/lhemala/dt/devices/dw1000"
"github.com/lhemala/dt/devices/dw1000/filter"
"github.com/lhemala/dt/devices/engine"
"github.com/lhemala/dt/devices/hokuyo"
"github.com/lhemala/dt/devices/sonic_sensors"
"github.com/lhemala/dt/gui"
"github.com/lhemala/dt/log"
"github.com/lhemala/dt/state"
)

var logger = log.New("main")

var _ = &engine.Engine{}
var _ = &sonic_sensors.SonicSensors{}
var _ = &arduino.Arduino{}
var _ = &dw1000.DefaultDW1000{}
var _ = &hokuyo.Hokuyo{}

func main() {
logger := logger.ForFunc("main")
logger.Debug("enter func")
defer logger.Debug("leave func")

arduino, err := arduino.New("/dev/ttyACM0", 38400)

if err != nil {
panic("failed to connect to arduino")
}

engine := engine.New(arduino)
sonics := sonic_sensors.New(arduino)

var laser *hokuyo.Hokuyo

for i := 0; i < 5; i++ {
laser, err = hokuyo.New("/dev/ttyACM1", -90, 90)
if err != nil {
time.Sleep(time.Second)
continue
}

break
}
if err != nil {
panic("laser sensor could not be initialised")
}

dw, err := dw1000.New(&dw1000.Config{

SerialPort: "/dev/ttyACM2",
AnchorLocation: [3]dw1000.Point{
{
X: 0,
 Y: 0,
},
{
X: 7.227,
Y: 0.299,
},
{
X: 0.358,
Y: 4.491,
},
},
FilterA0: filter.NewMedianFilter(5, filter.NewAverageFilter(5, nil)),
FilterA1: filter.NewMedianFilter(5, filter.NewAverageFilter(5, nil)),
FilterA2: filter.NewMedianFilter(5, filter.NewAverageFilter(5, nil)),
})
if err != nil {
panic("location sensor could not be initialised")
}

err = engine.Run()
if err != nil {
panic(err)
}

engine.SetAngle(0)
engine.SetSpeed(0)

dw.Run()

sm := state.NewStateManager(
state.WithDW1000(dw),
state.WithEngine(engine),
state.WithSonicSensors(sonics),
state.WithHokuyo(laser),
)

sm.Run()

//sm.AddDestination(&state.Location{
// X: 100,
// Y: 100,
//})
//sm.AddDestination(&state.Location{
// X: 250,
// Y: 250,
//})

ch := command_handler.NewCommandHandler(sm, engine)

api, trigger := api.NewApi(sm, ch)

go func() {
for i := 0; ; i++ {
trigger <- "0"

time.Sleep(time.Millisecond * 100)
}
}()

mux := http.NewServeMux()
 mux.HandleFunc("/", gui.Handle)
mux.Handle("/api", api.Handler)

s := &http.Server{
Addr: ":1717",
Handler: mux,
ReadTimeout: 10 * time.Second,
WriteTimeout: 10 * time.Second,
MaxHeaderBytes: 1 << 20,
}

compass, err := application.NewCompass(sm, 25.0)

if err != nil {
panic("compass generation failed")
}
app, err := application.NewApplication(sm, compass)
if err != nil {
panic("application generation failed")
}
go app.Run()

sm.SetMode(state.RemoteDriving)
//time.AfterFunc(time.Second*5, func()
{ sm.SetMode(state.AutonomousDriving) })

panic(s.ListenAndServe())
}