package gohumantrack

/*
#cgo CFLAGS: -I${SRCDIR}/sdk -w -g
#cgo LDFLAGS: -L/usr/local/cuda-10.0/lib64 -L${SRCDIR}/sdk/libs
#cgo LDFLAGS: -L${SRCDIR}/opencv-2.4.13/lib -L${SRCDIR}/TensorRT-5.1.2.2/lib
#cgo LDFLAGS: -Wl,-rpath,${SRCDIR}/sdk/libs:${SRCDIR}/TensorRT-5.1.2.2:${SRCDIR}/opencv-2.4.13
#cgo LDFLAGS: -lmidHumanTrack -lHumanTracker -lutools
#cgo LDFLAGS: -lcudart -lcublas -lcurand -lcusolver -lcudnn
#cgo LDFLAGS: -lnppc -lnppicom
#cgo LDFLAGS: -lnvrtc -lnvinfer -lnvinfer_plugin -lnvparsers -lnvonnxparser
#cgo LDFLAGS: -lopencv_video -lopencv_highgui -lopencv_imgproc -lopencv_core
#cgo LDFLAGS: -ldl -lpthread -lm

#include <stdlib.h>
#include "c_include/c_human_tracker.h"

void *create_batch_image(const int size){
	c_img *imgs = (c_img*)malloc(size * sizeof(c_img));
	for(int i = 0; i < size; i++){
		imgs[i].data_ = NULL;
	}
	return imgs;
}

void *create_fake_image(const int size){
	unsigned char *img = (unsigned char*)malloc(size);
	printf("batch~~~~~~ fake image len: %d\n", size);
	memset(img, 0, size);
	return img;
}

int fill_images(void *imgs, const int size, const int index, void *data, const int w, const int h, const int c){
	c_img *images = (c_img*)imgs;
	images[index].data_ = (unsigned char*)data;
	images[index].w_ = w;
	images[index].h_ = h;
	images[index].c_ = c;
	return index;
}

void *init_fgres(const int size){
	c_fgRet *ret = (c_fgRet*)malloc(size * sizeof(c_fgRet));
	for(int i = 0; i < size; i++){
		ret[i].fgNum = 0;
	}
	return ret;
}
int process(void *handle, void *imgs, const int size, void *result){
	c_img *images = (c_img*)imgs;
	for (int i = 0; i < size; i++){
		printf("batch~~~~~~C image index %d, address: %p, size: %dx%dx%d\n",
			i, images[i].data_, images[i].w_, images[i].h_, images[i].c_);
	}
	c_fgRet *res = (c_fgRet *)result;
	return c_human_tracker_process(handle, images, size, res);
}
*/
import "C"
import (
	"analysis/logo"
	"errors"
	"fmt"
	"math"
	"unsafe"
)

// ImageHumanTracker image
type ImageHumanTracker struct {
	Data    []byte
	Width   int
	Height  int
	Channel int
}

// int left;//行人包围框左上角横坐标
// int right;//行人包围框右下角横坐标
// int top;//行人包围框左上角纵坐标
// int bottom;//行人包围框右下角纵坐标
// float confidence;//行人检测框置信度得分
// int center_x;//行人包围框中心点x
// int center_y;//行人包围框中心点y
// int ID;//行人跟踪ID
// float feature[FEATURESIZE];//行人专属特征，可用来做ReID

// FgInfo info
type FgInfo struct {
	Left       int32
	Right      int32
	Top        int32
	Bottom     int32
	Confidence float32
	X          int32
	Y          int32
	ID         int32
	Feature    [128]float32
}

// fgInfo fginfo[MAX_BG_NUM];//上述结构体，代表一幅图中所有的框，最大支持2000个框，暂不可修改
// int fgNum;//一幅图中框的数量（fginfo中的有效框数）

// FgResult result
type FgResult struct {
	Fginfo [2000]FgInfo
	FgNum  int32
}

// HumanTracker struct
type HumanTracker struct {
	handle    unsafe.Pointer
	result    unsafe.Pointer
	batchSize int
}

// NewHumanTracker new
func NewHumanTracker(gpu, batchSize, flag int) *HumanTracker {
	if gpu == -1 {
		return nil
	}
	p := C.c_human_tracker_create(C.int(gpu), C.int(batchSize), C.int(flag))
	if p != nil {
		res := C.init_fgres(C.int(batchSize))
		return &HumanTracker{p, res, batchSize}
	}
	return nil
}

// Free free
func (h *HumanTracker) Free() {
	if h.handle != nil {
		C.c_human_tracker_release(&h.handle)
	}
	if h.result != nil {
		C.free(h.result)
	}
}

// GetVersion ver
func (h *HumanTracker) GetVersion() string {
	if h.handle == nil {
		return "human tracker handle null"
	}
	ver := C.c_getVersion()
	if ver == nil {
		return "get version string null"
	}
	return C.GoString(ver)
}

// Process process
func (h *HumanTracker) Process(imgs []ImageHumanTracker) ([]FgResult, error) {
	if len(imgs) != h.batchSize {
		return nil, errors.New("input images count doesn't equalize to batchsize")
	}
	cImgs := C.create_batch_image(C.int(h.batchSize))
	if cImgs == nil {
		return nil, errors.New("create C images error")
	}
	defer C.free(cImgs)
	for k, v := range imgs {
		ret := C.fill_images(cImgs, C.int(h.batchSize), C.int(k), unsafe.Pointer(&v.Data[0]), C.int(v.Width), C.int(v.Height), C.int(v.Channel))
		if int(ret) != k {
			return nil, errors.New("fill C images error")
		}
	}

	cRet := C.process(h.handle, cImgs, C.int(h.batchSize), h.result)
	if cRet != 0 {
		return nil, fmt.Errorf("process error: %d", int(cRet))
	}

	var result []FgResult
	p := uintptr(cRet)
	for i := 0; i < h.batchSize; i++ {
		j := *(*FgResult)(unsafe.Pointer(p))
		result = append(result, j)
		p += unsafe.Sizeof(j)
	}
	return result, nil
}

// ProcessImagePointer process
func (h *HumanTracker) ProcessImagePointer(imgs []*ImageHumanTracker, wid, hei, chn int) ([]FgResult, error) {
	if len(imgs) != h.batchSize {
		return nil, errors.New("input images count doesn't equalize to batchsize")
	}
	cImgs := C.create_batch_image(C.int(h.batchSize))
	if cImgs == nil {
		return nil, errors.New("create C images error")
	}
	defer C.free(cImgs)

	// var freeFakeImage []unsafe.Pointer
	// defer func(f []unsafe.Pointer) {
	// 	for _, v := range f {
	// 		C.free(v)
	// 	}
	// }(freeFakeImage)

	var fakeImage *ImageHumanTracker
	for _, v := range imgs {
		if fakeImage == nil && v != nil {
			fakeImage = v
			break
		}
	}
	if fakeImage == nil {
		logo.Infoln("batch~~~~~~ images slice no images")
		return nil, errors.New("images slice is null")
	}
	for k, v := range imgs {
		if v == nil {
			// fake := C.create_fake_image(C.int(wid * hei * chn))
			// freeFakeImage = append(freeFakeImage, fake)
			// C.fill_images(cImgs, C.int(h.batchSize), C.int(k), fake, C.int(wid), C.int(hei), C.int(chn))

			v = fakeImage
			logo.Infoln("batch~~~~~~ fake image ", k, " len: ", len(v.Data), " size: ", v.Width, "x", v.Height, "x", v.Channel)
			C.fill_images(cImgs, C.int(h.batchSize), C.int(k), unsafe.Pointer(&v.Data[0]), C.int(v.Width), C.int(v.Height), C.int(v.Channel))

		} else {
			logo.Infoln("batch~~~~~~ real image ", k, " len: ", len(v.Data), " size: ", v.Width, "x", v.Height, "x", v.Channel)
			C.fill_images(cImgs, C.int(h.batchSize), C.int(k), unsafe.Pointer(&v.Data[0]), C.int(v.Width), C.int(v.Height), C.int(v.Channel))
		}
	}

	cRet := C.process(h.handle, cImgs, C.int(h.batchSize), h.result)
	if cRet != 0 {
		return nil, fmt.Errorf("process image pointer error: %d", int(cRet))
	}

	var result []FgResult
	p := uintptr(h.result)
	for i := 0; i < h.batchSize; i++ {
		j := *(*FgResult)(unsafe.Pointer(p))
		result = append(result, j)
		p += unsafe.Sizeof(j)
	}
	return result, nil
}

// FFSimilarity similarity
func FFSimilarity(feaA, feaB [128]float32) float64 {
	var norm1, norm2 float64
	var score float64

	for i := 0; i < 128; i++ {
		norm1 += float64(feaA[i]) * float64(feaA[i])
		norm2 += float64(feaB[i]) * float64(feaB[i])
		score += float64(feaA[i]) * float64(feaB[i])
	}

	norm1 = math.Sqrt(norm1)
	norm2 = math.Sqrt(norm2)

	score = score / (norm1 * norm2)

	if score < 0 {
		score = 0
	}
	return score

}

// CFFSimilarity use c code
func CFFSimilarity(feaA, feaB []float32) (float64, error) {
	if len(feaA) != len(feaB) {
		return 0, errors.New("Must be same length")
	}
	if len(feaA) != 128 || len(feaB) != 128 {
		return 0, errors.New("Must be 128 size")
	}

	s := C.c_FF_Similarity((*C.float)(&feaA[0]), (*C.float)(&feaB[0]))
	return float64(s), nil
}