using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace PSO
{
    class Program
    {
        static Random rnd = new Random();

        static void Main(string[] args)
        {
            Console.WriteLine("Particle Swarm Optimization in C#");

            RunPSO();

        }

        static double Sphere(double[] x)
        {
            double S = 0;
            for (int i = 0; i < x.Length; i++)
            {
                S += Math.Pow(x[i], 2);
            }
            return S;
        }

        static void RunPSO()
        {
            // Problem Parameters
            Func<double[], double> ObjectiveFunction = Sphere;
            ObjectiveType ObjType = ObjectiveType.Minimization;
            int nVar = 10;
            double LB = 1;
            double UB = 10;

            // PSO Parameters
            int nPop = 100;
            int nMaxIter = 1000;

            double w, wdamp, c1, c2;

            // Config 1
            /*
            w = 1;
            wdamp = 0.99;
            c1 = 2;
            c2 = 2;
            */

            // Config 2 (Constriction Coefficients)
            double kappa = 1;
            double phi1 = 2.05;
            double phi2 = 2.05;
            double phi = phi1 + phi2;
            double chi = 2 * kappa / (phi - 2 + Math.Sqrt(phi * phi - 4 * phi));
            w = chi;
            wdamp = 1;
            c1 = chi * phi1;
            c2 = chi * phi2;

            // Initialization of Global Best
            Particle GlobalBest;
            GlobalBest.Position = new double[nVar];
            if(ObjType == ObjectiveType.Minimization)
            {
                GlobalBest.ObjectiveValue = double.PositiveInfinity;
            }
            else
            {
                GlobalBest.ObjectiveValue = double.NegativeInfinity;
            }

            // Initialization of Population
            Particle[] pop = new Particle[nPop];
            for (int i = 0; i < nPop; i++)
            {
                pop[i].Position = rand(LB, UB, nVar);
                pop[i].Velocity = new double[nVar];
                pop[i].ObjectiveValue = ObjectiveFunction(pop[i].Position);
                pop[i].BestPosition = (double[])pop[i].Position.Clone();
                pop[i].BestObjectiveValue = pop[i].ObjectiveValue;

                if(IsBetter(pop[i].BestObjectiveValue, GlobalBest.ObjectiveValue, ObjType))
                {
                    GlobalBest.Position = (double[])pop[i].BestPosition.Clone();
                    GlobalBest.ObjectiveValue = pop[i].BestObjectiveValue;
                }
            }

            // PSO Main Loop
            for (int iter = 0; iter < nMaxIter; iter++)
            {
                for (int i = 0; i < nPop; i++)
                {
                    for (int j = 0; j < nVar; j++)
                    {
                        // Update Velocity
                        pop[i].Velocity[j] = w * pop[i].Velocity[j]
                            + c1 * rand() * (pop[i].BestPosition[j] - pop[i].Position[j])
                            + c2 * rand() * (GlobalBest.Position[j] - pop[i].Position[j]);

                        // Update Position
                        pop[i].Position[j] = pop[i].Position[j] + pop[i].Velocity[j];

                        // Apply Lower and Upper Bounds
                        pop[i].Position[j] = Math.Max(pop[i].Position[j], LB);
                        pop[i].Position[j] = Math.Min(pop[i].Position[j], UB);

                    }

                    pop[i].ObjectiveValue = ObjectiveFunction(pop[i].Position);

                    if(IsBetter(pop[i].ObjectiveValue, pop[i].BestObjectiveValue, ObjType))
                    {
                        pop[i].BestPosition = (double[])pop[i].Position.Clone();
                        pop[i].BestObjectiveValue = pop[i].ObjectiveValue;

                        if (IsBetter(pop[i].BestObjectiveValue, GlobalBest.ObjectiveValue, ObjType))
                        {
                            GlobalBest.Position = (double[])pop[i].BestPosition.Clone();
                            GlobalBest.ObjectiveValue = pop[i].BestObjectiveValue;
                        }
                    }
                }

                Console.WriteLine("Iteration {0}: Best Objective = {1}", iter + 1, GlobalBest.ObjectiveValue);

                // Damping Intertia Rate
                w *= wdamp;

            }

        }

        static double rand()
        {
            return rnd.NextDouble();
        }

        static double rand(double a, double b)
        {
            return a + (b - a) * rand();
        }

        static double[] rand(double a, double b, int n)
        {
            double[] x = new double[n];
            for (int i = 0; i < n; i++)
            {
                x[i] = rand(a, b);
            }
            return x;
        }

        static bool IsBetter(double ObjectiveValue1, double ObjectiveValue2, ObjectiveType ObjType)
        {
            if(ObjType == ObjectiveType.Minimization)
            {
                return (ObjectiveValue1 <= ObjectiveValue2);
            }
            else
            {
                return (ObjectiveValue1 >= ObjectiveValue2);
            }
        }
    }


    struct Particle
    {
        public double[] Position;
        public double[] Velocity;
        public double ObjectiveValue;
        public double[] BestPosition;
        public double BestObjectiveValue;
    }

    enum ObjectiveType
    {
        Minimization, Maximization
    }
}
