package producer

import (
	"encoding/json"
	"fmt"
	"os"
	"strings"

	"github.com/paulmach/orb"
	"github.com/paulmach/orb/geojson"

	log "github.com/sirupsen/logrus"
	"gonum.org/v1/gonum/mat"
	"starwiz.cn/sjy01/image-proc/pkg/dem"
)

const FLT_EPSILON = 1.192092896e-07

type RPC struct {
	lineOffset, lineScale               float64
	sampOffset, sampScale               float64
	latOffset, longOffset, heightOffset float64
	latScale, longScale, heightScale    float64

	LineCoef RPCModel
	SampCoef RPCModel

	minH, maxH     float64
	GCPs           []*GroundPoint
	elevationLayer int
	gridsize       int

	scene       *Scene
	registrator *Registrator

	rpb string
}

// GroundPoint 表示地面点的三维坐标
type GroundPoint struct {
	P, L, H float64 // P-latitude, L-longitude, H-height
	Y, X    float64 // X-sample, Y-line
}

// ImagePoint 表示像素平面上的点
type ImagePoint struct {
	Y, X float64 // X-sample, Y-line
}

// RPCModel 包含20个系数的RPC模型
type RPCModel struct {
	NumCoefficients [20]float64 // 分子系数
	DenCoefficients [20]float64 // 分母系数
}

// rational polynomial coeffients
func NewRPC(r *Registrator, scene *Scene, rpb string) *RPC {
	rpc := RPC{
		elevationLayer: 3,
		gridsize:       3,
		registrator:    r,
		scene:          scene,
		rpb:            rpb,
	}

	log.Info("start RPC initialization for scene: ", scene.Tiff)
	rpc.init()

	return &rpc
}

// 初始化经纬度
func (rpc *RPC) init() {
	rpc.minH = 9999.0
	rpc.maxH = -9999.0

	rpc.minH, rpc.maxH = dem.Dem1KmLT.MinMaxElevationInRect(
		rpc.scene.Meta.Corners.UpperLeft.Longitude,
		rpc.scene.Meta.Corners.UpperLeft.Latitude,
		rpc.scene.Meta.Corners.LowerRight.Longitude,
		rpc.scene.Meta.Corners.LowerRight.Latitude,
	)

	if rpc.minH < -10.0 {
		rpc.minH = 0.0
	}

	if rpc.maxH < -10.0 {
		rpc.maxH = 0.0
	}

}

// 虚拟控制点
func (rpc *RPC) generateVirtualGCP() {
	points := gridImage2(rpc.gridsize, rpc.gridsize,
		rpc.scene.Height, rpc.scene.Width,
		rpc.elevationLayer, int(rpc.minH), int(rpc.maxH))

	for _, p := range points {
		p84 := rpc.registrator.calculateLatLonH(rpc.scene, p.Row, p.Col, p.H)
		rpc.GCPs = append(rpc.GCPs, &GroundPoint{
			P: p84.Lat,
			L: p84.Lon,
			H: p84.H,
			Y: float64(p.Row),
			X: float64(p.Col),
		})
	}

	name := strings.Replace(rpc.scene.Tiff, ".tiff", ".gcp.geojson", -1)
	rpc.saveGCP(rpc.GCPs, name)
	name = strings.Replace(rpc.scene.Tiff, ".tiff", ".gcp_orig.txt", -1)
	rpc.saveGCP2(rpc.GCPs, name)
}

func (rpc *RPC) RPC() error {
	rpc.generateVirtualGCP()
	n := len(rpc.GCPs)
	log.Info("num of virtual GCPs: ", n)

	rowVec := mat.NewVecDense(n, nil)
	colVec := mat.NewVecDense(n, nil)
	latVec := mat.NewVecDense(n, nil)
	lonVec := mat.NewVecDense(n, nil)
	heightVec := mat.NewVecDense(n, nil)

	for i, ip := range rpc.GCPs {
		rowVec.SetVec(i, ip.Y)
		colVec.SetVec(i, ip.X)
		latVec.SetVec(i, ip.P)
		lonVec.SetVec(i, ip.L)
		heightVec.SetVec(i, ip.H)
	}

	rpc.saveVec(strings.Replace(rpc.scene.Tiff, ".tiff", ".vec.txt", -1),
		rowVec, colVec, latVec, lonVec, heightVec)

	rpc.lineOffset = float64(rpc.scene.Height) / 2.0
	rpc.lineScale = float64(rpc.scene.Height) / 2.0
	rpc.sampOffset = float64(rpc.scene.Width) / 2.0
	rpc.sampScale = float64(rpc.scene.Width) / 2.0
	rowVec = normalize2(rowVec, rpc.lineOffset, rpc.lineScale)
	colVec = normalize2(colVec, rpc.sampOffset, rpc.sampScale)

	latVec, rpc.latOffset, rpc.latScale = normalize(latVec)
	lonVec, rpc.longOffset, rpc.longScale = normalize(lonVec)
	heightVec, rpc.heightOffset, rpc.heightScale = normalize(heightVec)

	rpc.saveVec(strings.Replace(rpc.scene.Tiff, ".tiff", ".vec_norm.txt", -1),
		rowVec, colVec, latVec, lonVec, heightVec)

	// 设计矩阵 B = [ 20个分子系数 19个分母系数 ]

	// x = (B^T * B)^-1 * B^T * l, 其中 x = [a1..a20 b2..b20]^T
	// 行参数
	// B := setupSystemOfEquations(rowVec, latVec, lonVec, heightVec)
	// J, err := SolveNormalEquation(B, rowVec)
	log.Println("solving row coefficients")
	J, err := solveCoefficients(rowVec, latVec, lonVec, heightVec, SolveMethodNelderMead)
	if err != nil {
		return err
	}

	// 列参数
	// D := setupSystemOfEquations(colVec, latVec, lonVec, heightVec)
	// K, err := SolveNormalEquation(D, colVec)
	log.Println("solving col coefficients")
	K, err := solveCoefficients(colVec, latVec, lonVec, heightVec, SolveMethodNelderMead)
	if err != nil {
		return err
	}

	rpc.LineCoef.NumCoefficients[0] = J[0]
	rpc.LineCoef.DenCoefficients[0] = 1.0
	rpc.SampCoef.NumCoefficients[0] = K[0]
	rpc.SampCoef.DenCoefficients[0] = 1.0
	for i := 1; i < 20; i++ {
		rpc.LineCoef.NumCoefficients[i] = J[i]
		rpc.LineCoef.DenCoefficients[i] = J[i+19]
		rpc.SampCoef.NumCoefficients[i] = K[i]
		rpc.SampCoef.DenCoefficients[i] = K[i+19]
	}

	nameRPB0 := strings.Replace(rpc.scene.Tiff, ".tiff", ".rpb", -1)
	rpc.saveRPB(nameRPB0)

	projectedPoints := rpc.applyRFM(
		mat.NewVecDense(20, rpc.LineCoef.NumCoefficients[:]),
		mat.NewVecDense(20, rpc.LineCoef.DenCoefficients[:]),
		mat.NewVecDense(20, rpc.SampCoef.NumCoefficients[:]),
		mat.NewVecDense(20, rpc.SampCoef.DenCoefficients[:]),
		rpc.GCPs,
	)
	name := strings.Replace(rpc.scene.Tiff, ".tiff", ".gcp_proj.txt", -1)
	rpc.saveGCP2(projectedPoints, name)

	return nil
}

func (rpc *RPC) Output() string {
	var lineNumCoef, lineDenCoef, sampNumCoef, sampDenCoef string
	for i := 0; i < 20; i++ {
		if i < 19 {
			lineNumCoef += fmt.Sprintf("\t\t%.15e,\n", rpc.LineCoef.NumCoefficients[i])
			lineDenCoef += fmt.Sprintf("\t\t%.15e,\n", rpc.LineCoef.DenCoefficients[i])
			sampNumCoef += fmt.Sprintf("\t\t%.15e,\n", rpc.SampCoef.NumCoefficients[i])
			sampDenCoef += fmt.Sprintf("\t\t%.15e,\n", rpc.SampCoef.DenCoefficients[i])
		} else {
			lineNumCoef += fmt.Sprintf("\t\t%.15e", rpc.LineCoef.NumCoefficients[i])
			lineDenCoef += fmt.Sprintf("\t\t%.15e", rpc.LineCoef.DenCoefficients[i])
			sampNumCoef += fmt.Sprintf("\t\t%.15e", rpc.SampCoef.NumCoefficients[i])
			sampDenCoef += fmt.Sprintf("\t\t%.15e", rpc.SampCoef.DenCoefficients[i])
		}
	}

	model := fmt.Sprintf(`satId = "SJY01";
bandId = "";
SpecId = "";
BEGIN_GROUP = IMAGE
	errBias = 1.0;
	errRand = 0.0;
	lineOffset = %.8f;
	sampOffset = %.8f;
	latOffset = %.8f;
	longOffset = %.8f;
	heightOffset = %.8f;
	lineScale = %.8f;
	sampScale = %.8f;
	latScale = %.8f;
	longScale = %.8f;
	heightScale = %.8f;
	lineNumCoef = (
%s);
	lineDenCoef = (
%s);
	sampNumCoef = (
%s);
	sampDenCoef = (
%s);
END_GROUP = IMAGE
END;
`,
		rpc.lineOffset, rpc.sampOffset, rpc.latOffset, rpc.longOffset, rpc.heightOffset,
		rpc.lineScale, rpc.sampScale, rpc.latScale, rpc.longScale, rpc.heightScale,
		lineNumCoef, lineDenCoef, sampNumCoef, sampDenCoef,
	)

	return model
}

func (rpc *RPC) saveRPB(name string) error {
	log.Infof("save RPC model to %s", name)
	model := rpc.Output()
	f, err := os.Create(name)
	if err != nil {
		log.Errorf("Failed to create RPC model file: %v", err)
		return err
	}
	defer f.Close()
	_, err = f.WriteString(model)
	if err != nil {
		log.Errorf("Failed to write RPC model file: %v", err)
	}
	f.Sync()
	return err
}

func (rpc *RPC) saveGCP(gcps []*GroundPoint, name string) error {
	log.Infof("save gcp to %s", name)
	f, err := os.Create(name)
	if err != nil {
		log.Errorf("Failed to create GCP file: %v", err)
		return err
	}
	defer f.Close()

	var gcp geojson.FeatureCollection
	for _, p := range gcps {
		point := orb.Point{p.L, p.P}
		feature := geojson.NewFeature(point)
		feature.Properties = map[string]interface{}{
			"H": p.H,
			"Y": p.Y,
			"X": p.X,
		}
		gcp.Features = append(gcp.Features, feature)
	}
	data, _ := json.Marshal(gcp)
	f.Write(data)
	f.Sync()

	return nil
}

func (rpc *RPC) saveGCP2(gcps []*GroundPoint, name string) error {
	log.Infof("save gcp to %s", name)
	f, err := os.Create(name)
	if err != nil {
		log.Errorf("Failed to create GCP file: %v", err)
		return err
	}
	defer f.Close()

	for _, p := range gcps {
		f.WriteString(fmt.Sprintf("%.8f\t%.8f\t%.8f\t%.8f\t%.8f\n", p.L, p.P, p.H, p.Y, p.X))
	}
	f.Sync()

	return nil
}

func (rpc *RPC) saveVec(name string, rowVec, colVec, latVec, lonVec, heightVec *mat.VecDense) error {
	f, err := os.Create(name)

	if err != nil {
		log.Errorf("Failed to create vec file: %v", err)
		return err
	}
	defer f.Close()

	for i := 0; i < rowVec.Len(); i++ {
		f.WriteString(fmt.Sprintf("%.8f\t%.8f\t%.8f\t%.8f\t%.8f\n",
			rowVec.AtVec(i), colVec.AtVec(i), latVec.AtVec(i), lonVec.AtVec(i), heightVec.AtVec(i)))
	}
	return nil
}

func (rpc *RPC) applyRFM(num_line, den_line, num_samp, den_samp *mat.VecDense, points []*GroundPoint) []*GroundPoint {
	var res []*GroundPoint
	for _, p := range points {
		var r GroundPoint
		P := (p.P - rpc.latOffset) / rpc.latScale
		L := (p.L - rpc.longOffset) / rpc.longScale
		H := (p.H - rpc.heightOffset) / rpc.heightScale
		r.Y = project(num_line, den_line, P, L, H)
		r.X = project(num_samp, den_samp, P, L, H)
		r.P = p.P
		r.L = p.L
		r.H = p.H
		r.Y = (r.Y*rpc.lineScale + rpc.lineOffset)
		r.X = (r.X*rpc.sampScale + rpc.sampOffset)
		res = append(res, &r)
	}

	return res
}