package main

import (
	"flag"
	"github.com/cyrilix/robocar-base/cli"
	"github.com/cyrilix/robocar-road/road"
	"go.uber.org/zap"
	"gocv.io/x/gocv"
	"image"
	"log"
	"math"
	"os"
)

const (
	DefaultClientId = "robocar-road"
	DefaultHorizon  = 110
)

func main() {
	var mqttBroker, username, password, clientId string
	var cameraTopic, roadTopic string
	var horizon int
	var whiteThresholdLow, whiteThresholdHigh int
	var cannyThresholdLow, cannyThresholdHigh int
	var imgWidth, imgHeight int

	err := cli.SetIntDefaultValueFromEnv(&horizon, "HORIZON", DefaultHorizon)
	if err != nil {
		log.Printf("unable to parse horizon value arg: %v", err)
	}

	mqttQos := cli.InitIntFlag("MQTT_QOS", 0)
	_, mqttRetain := os.LookupEnv("MQTT_RETAIN")

	cli.InitMqttFlags(DefaultClientId, &mqttBroker, &username, &password, &clientId, &mqttQos, &mqttRetain)

	flag.StringVar(&roadTopic, "mqtt-topic-road", os.Getenv("MQTT_TOPIC_ROAD"), "Mqtt topic to publish road detection result, use MQTT_TOPIC_ROAD if args not set")
	flag.StringVar(&cameraTopic, "mqtt-topic-camera", os.Getenv("MQTT_TOPIC_CAMERA"), "Mqtt topic that contains camera frame values, use MQTT_TOPIC_CAMERA if args not set")
	flag.IntVar(&horizon, "horizon", horizon, "Limit horizon in pixels from top, use HORIZON if args not set")

	flag.IntVar(&imgWidth, "image-width", 160, "Video pixels width")
	flag.IntVar(&imgHeight, "image-height", 120, "Video pixels height")

	flag.IntVar(&whiteThresholdLow, "white-threshold-low", 20, "White pixels threshold, low limit")
	flag.IntVar(&whiteThresholdHigh, "white-threshold-high", 255, "White pixels threshold, high limit")

	flag.IntVar(&cannyThresholdLow, "canny-threshold-low", 100, "White pixels threshold, low limit")
	flag.IntVar(&cannyThresholdHigh, "canny-threshold-high", 250, "White pixels threshold, high limit")

	var houghLinesRho, houghLinesThreshold, houghLinesMinLineLength, houghLinesMaxLineGap int
	var houghLinesTheta float64
	flag.IntVar(&houghLinesRho, "hough-lines-rho", 2, "distance resolution in pixels of the Hough grid")
	flag.Float64Var(&houghLinesTheta, "hough-lines-theta", 1*math.Pi/180, "angular resolution in radians of the Hough grid, default Pi/180")
	flag.IntVar(&houghLinesThreshold, "hough-lines-threshold", 15, "minimum number of votes (intersections in Hough grid cell)")
	flag.IntVar(&houghLinesMinLineLength, "hough-lines-min-line-length", 10, "minimum number of pixels making up a line")
	flag.IntVar(&houghLinesMaxLineGap, "hough-lines-max-lines-gap", 20, "maximum gap in pixels between connectable line segments")

	logLevel := zap.LevelFlag("log", zap.InfoLevel, "log level")
	flag.Parse()

	if len(os.Args) <= 1 {
		flag.PrintDefaults()
		os.Exit(1)
	}

	config := zap.NewDevelopmentConfig()
	config.Level = zap.NewAtomicLevelAt(*logLevel)
	lgr, err := config.Build()
	if err != nil {
		log.Fatalf("unable to init logger: %v", err)
	}
	defer func() {
		if err := lgr.Sync(); err != nil {
			log.Printf("unable to Sync logger: %v\n", err)
		}
	}()
	zap.ReplaceGlobals(lgr)

	zap.S().Infof("topic road                 : %v", roadTopic)
	zap.S().Infof("topic camera               : %v", cameraTopic)
	zap.S().Infof("horizon                    : %v", horizon)
	zap.S().Infof("image width                : %v", imgWidth)
	zap.S().Infof("image height               : %v", imgHeight)
	zap.S().Infof("white threshold low        : %v", whiteThresholdLow)
	zap.S().Infof("white threshold high       : %v", whiteThresholdHigh)
	zap.S().Infof("canny threshold low        : %v", cannyThresholdLow)
	zap.S().Infof("canny threshold high       : %v", cannyThresholdHigh)
	zap.S().Infof("hough lines rho            : %v", houghLinesRho)
	zap.S().Infof("hough lines theta          : %v", houghLinesTheta)
	zap.S().Infof("hough lines threshold      : %v", houghLinesThreshold)
	zap.S().Infof("hough lines min line length: %v", houghLinesMinLineLength)
	zap.S().Infof("hough lines max line gap   : %v", houghLinesMaxLineGap)

	client, err := cli.Connect(mqttBroker, username, password, clientId)
	if err != nil {
		zap.S().Fatalf("unable to connect to mqtt bus: %v", err)
	}
	defer client.Disconnect(50)

	monitor := &road.DefaultMonitor{}
	go monitor.Start()

	p := road.NewPart(client,
		cameraTopic, roadTopic,
		road.NewDetector(
			road.WithWhiteFilter(whiteThresholdLow, whiteThresholdHigh),
			road.WithYellowFilter(
				gocv.NewMatFromScalar(gocv.Scalar{Val1: 90., Val2: 100., Val3: 100.}, gocv.MatTypeCV8U),
				gocv.NewMatFromScalar(gocv.Scalar{Val1: 110., Val2: 255., Val3: 255.}, gocv.MatTypeCV8U),
			),
			road.WithCanny(cannyThresholdLow, cannyThresholdHigh),
			road.WithGaussianBlur(3),
			road.WithRegionOfInterest(imgWidth, imgHeight, horizon),
			road.WithPointOnRoad(image.Point{X: imgWidth / 2, Y: imgHeight - 30}),
			road.WithHoughLines(houghLinesRho, float32(houghLinesTheta), houghLinesThreshold, houghLinesMinLineLength, houghLinesMaxLineGap),
			road.WithMonitor(monitor),
		),
	)
	defer p.Stop()

	cli.HandleExit(p)

	err = p.Start()
	if err != nil {
		zap.S().Fatalf("unable to start service: %v", err)
	}
}