package soko

func Solve(l Level) int {
	state := l.State()
	return solve(state)
}

func solve(state State) int {
	level := state.Level
	target := state.Target.Pos
	costs := &stateCostQueue{}
	costs.Put(&stateCost{&state, 0, nil})
	costsByPlayerIdx := make([][]*stateCost, level.Size())
	findCost := func(s *State) *stateCost {
		costs := costsByPlayerIdx[s.Player.Pos]
		for i := 0; i < len(costs); i++ {
			if !costs[i].state.Equal(s) {
				continue
			}
			return costs[i]
		}
		return nil
	}
	addCost := func(next *State, nextCost int) {
		cost := findCost(next)
		if cost == nil {
			newCost := &stateCost{next, nextCost, nil}
			costs.Put(newCost)
			player := next.Player.Pos
			costsByPlayerIdx[player] = append(costsByPlayerIdx[player], newCost)
		} else if nextCost < cost.cost {
			cost.cost = nextCost
			costs.Update(cost)
		}
	}
	moves := level.Moves()
	type direction struct {
		dir     Direction
		inverse Direction
	}
	dirs := make([]direction, 4)
	for _, dir := range Directions {
		dirs[dir] = direction{dir, dir.Invert()}
	}
	for {
		curr := costs.Get()
		currPlayerPos := curr.state.Player.Pos
		if currPlayerPos == target {
			return curr.cost
		}

		if curr.distances == nil {
			curr.distances = NewPathFinderMoves(curr.state, moves).FindDistances()
		}
		distances := curr.distances
		for _, e := range curr.state.Entities {
			idx := e.Pos
			if e.Typ != EntityTypeBrick || curr.state.SunkenBricks[idx] == e.ID {
				continue
			}
			for i := 0; i < 4; i++ {
				player := moves[e.Pos].All[dirs[i].inverse]
				if player == -1 {
					continue
				}
				walk := distances[player]
				if walk == -1 {
					continue
				}
				if !curr.state.IsOpenForBrick(curr.state.IdxToPos[moves[e.Pos].All[i]]) {
					continue
				}
				curr.state.Player.Pos = player
				next, ok := curr.state.MovePlayer(dirs[i].dir)
				if !ok {
					panic("should be a valid move")
				}
				nextCost := curr.cost + walk + 1
				addCost(&next, nextCost)
			}
		}
		curr.state.Player.Pos = currPlayerPos
		if walk := distances[target]; walk != -1 {
			next := curr.state.Clone()
			next.Player.Pos = target
			nextCost := curr.cost + walk
			addCost(&next, nextCost)
		}

		if costs.Empty() {
			return -1
		}
	}
}