namespace NathanBrixius.AssignmentLib
{
    /// <summary>Linear assignment problem solver.<summary>
    /// <remarks>
    /// This code is a port of Fortran code by R.E. Burkard and U. Derigs. 
    /// Assignment and matching problems: Solution methods with fortran programs, 
    /// volume 184 of Lecture Notes in Economics and Mathematical Systems. 
    /// Springer, Berlin, 1980.  The original code is available on the QAPLIB site.
    /// </remarks>
    public class LinearAssignment
    {
        public static void Main(params string[] args)
        {
            double[][] L = new double[5][];
            for (int i = 0; i < L.Length; i++)
            {
                L[i] = new double[5];
                for (int j = 0; j < L[i].Length; j++)
                {
                    L[i][j] = i + j;
                }
            }
            int[] p = new int[L.Length];
            int[] ip = new int[L.Length];
            double[] ys = new double[L.Length];
            double[] yt = new double[L.Length];
            Solve(L, p, ip, ys, yt);
        }

        /// <summary>Evaluate a permutation against the given LAP.<summary>
        /// <param name="L">LAP cost matrix.</param>
        /// <param name="p">Permutation.</param>
        /// <returns>The cost of the permutation.</returns>
        public static double Evaluate(double[][] L, int[] p)
        {
            double cost = 0.0;
            for (int i = 0; i < p.Length; i++)
            {
                cost += L[i][p[i]];
            }
            return cost;
        }

        /// <summary>Calculate the reduced costs matrix for the given LAP and solution.<summary>
        /// <param name="L">LAP cost matrix.</param>
        /// <param name="U">Reduced cost matrix (filled in by the method)</param>
        /// <param name="ys">Dual variables.</param>
        /// <param name="yt">Dual variables.</param>
        public static void ReducedCosts(
            double[][] L,
            double[][] U,
            double[] ys,
            double[] yt)
        {
            for (int i = 0; i < L.Length; i++)
            {
                for (int j = 0; j < L[i].Length; j++)
                {
                    U[i][j] = L[i][j] - (yt[j] + ys[i]);

                }
            }
        }

        /// <summary>Solve the specified linear assignment problem.<summary>
        /// <param name="L">LAP cost matrix.</param>
        /// <param name="p">The solution permutation.</param>
        /// <param name="ip">The inverse of the solution permutation.</param>
        /// <param name="ys">Dual variables.</param>
        /// <param name="yt">Dual variables.</param>
        /// <returns></returns>
        public static double Solve(
            double[][] L,
            int[] p,
            int[] ip,
            double[] ys,
            double[] yt
            )
        {
            if (L.Length == 0)
            {
                return 0.0;
            }
            return SolveCore(L, ip, p, ys, yt, 1.0e-7);
        }

        /// <summary>Linear sum assignment problem with double precision costs.<summary>
        /// <param name="L">Cost matrix.</param>
        /// <param name="ip">Inverse of optimal assignment.</param>
        /// <param name="p">Optimal assignment.</param>
        /// <param name="ys">Optimal dual (row) variables.</param>
        /// <param name="yt">Optimal dual (column) variables.</param>
        /// <param name="tolerance">Machine accuracy.</param>
        /// <returns>The optimal cost.</returns>
        private static double SolveCore(
            double[][] L,
            int[] ip,
            int[] p,
            double[] ys,
            double[] yt,
            double tolerance
            )
        {
            const int BadIndex = -1;
            int[] vor = new int[L.Length];
            bool[] label = new bool[L.Length];
            double[] dMinus = new double[L.Length];
            double[] dPlus = new double[L.Length];

            // Start: Construction of an initial (partial) assignment.
            // (partial --> p[i]/ip[j] may be BadIndex for some i,j)
            double maxValue = 1.0e15;
            for (int i = 0; i < p.Length; i++)
            {
                ip[i] = BadIndex;
                p[i] = BadIndex;
                vor[i] = BadIndex;
                ys[i] = 0;
                yt[i] = 0;
            }

            double ui = 0.0;
            int jOpt = -1; // optimal j index
            for (int i = 0; i < L.Length; i++)
            {
                for (int j = 0; j < L[i].Length; j++)
                {
                    if ((j == 0) || (L[i][j] - ui < tolerance))
                    {
                        ui = L[i][j];
                        jOpt = j;
                    }
                }

                // guarantee: jOpt >= 0
                ys[i] = ui;
                if (ip[jOpt] == BadIndex)
                {
                    ip[jOpt] = i;
                    p[i] = jOpt;
                }
            }

            for (int j = 0; j < ip.Length; j++)
            {
                if (ip[j] == BadIndex)
                {
                    yt[j] = maxValue;
                }
            }

            for (int i = 0; i < ys.Length; i++)
            {
                ui = ys[i];
                for (int j = 0; j < yt.Length; j++)
                {
                    double vj = yt[j];
                    if (vj > tolerance)
                    {
                        double cc = L[i][j] - ui;
                        if (cc + tolerance < vj)
                        {
                            yt[j] = cc;
                            vor[j] = i;
                        }
                    }
                }
            }

            for (int j = 0; j < vor.Length; j++)
            {
                int i = vor[j];
                if ((i != BadIndex) && (p[i] == BadIndex))
                {
                    p[i] = j;
                    ip[j] = i;
                }
            }

            for (int i = 0; i < p.Length; i++)
            {
                if (p[i] == BadIndex)
                {
                    ui = ys[i];
                    for (int j = 0; j < ip.Length; j++)
                    {
                        if (ip[j] == BadIndex)
                        {
                            double lij = L[i][j];
                            if ((lij - ui - yt[j] + tolerance) <= 0)
                            {
                                p[i] = j;
                                ip[j] = i;
                            }
                        }
                    }
                }
            }

            // Construction of the optimal assignment
            double d = 0.0;
            int w = BadIndex;
            int index = BadIndex;
            for (int u = 0; u < p.Length; u++)
            {
                if (p[u] == BadIndex)
                {
                    // Shortest path computation
                    for (int i = 0; i < vor.Length; i++)
                    {
                        vor[i] = u;
                        label[i] = false;
                        dPlus[i] = maxValue;
                        dMinus[i] = L[u][i] - ys[u] - yt[i];
                    }
                    dPlus[u] = 0;

                    bool done = false;
                    while (!done)
                    {
                        d = maxValue;
                        for (int i = 0; i < dMinus.Length; i++)
                        {
                            if (!label[i] && (dMinus[i] + tolerance < d))
                            {
                                d = dMinus[i];
                                index = i;
                            }
                        }

                        // note: index >= 0
                        if (ip[index] != BadIndex)
                        {
                            label[index] = true;
                            w = ip[index];
                            dPlus[w] = d;
                            for (int i = 0; i < label.Length; i++)
                            {
                                if (!label[i])
                                {
                                    double vgl = d + L[w][i] - ys[w] - yt[i];
                                    if (dMinus[i] > vgl + tolerance)
                                    {
                                        dMinus[i] = vgl;
                                        vor[i] = w;
                                    }
                                }
                            }
                        }
                        else
                        {
                            done = true;
                        }
                    }

                    // Augmentation
                    done = false;
                    while (!done)
                    {
                        w = vor[index];
                        ip[index] = w;
                        int tempIndex = p[w];
                        p[w] = index;
                        if (w == u)
                        {
                            done = true;
                        }
                        else
                        {
                            index = tempIndex;
                        }
                    }

                    // Transformation
                    for (int i = 0; i < dPlus.Length; i++)
                    {
                        if (dPlus[i] != maxValue)
                        {
                            ys[i] += d - dPlus[i];
                        }

                        if (dMinus[i] + tolerance < d)
                        {
                            yt[i] += dMinus[i] - d;
                        }
                    }
                }
            }

            // Computation of the optimal value
            return Evaluate(L, p);
        }
    }
}