using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using HeuristicLab.Persistence.Default.CompositeSerializers.Storable;
using HeuristicLab.Common;
using HeuristicLab.Data;
using HeuristicLab.Parameters;
using HeuristicLab.PDPSimulation.DomainModel;

namespace HeuristicLab.PDPSimulation.DistanceMeasures {
  [StorableClass]
  public class DistanceMatrixMeasure: DistanceMeasure {
    private ValueParameter<DoubleMatrix> DistanceMatrixParameter {
      get { return (ValueParameter<DoubleMatrix>)Parameters["DistanceMatrix"]; }
    }

    private ValueParameter<IntMatrix> PointMappingParameter {
      get { return (ValueParameter<IntMatrix>)Parameters["PointMapping"]; }
    }

    private OptionalValueParameter<StringArray> PointNamesParameter {
      get { return (OptionalValueParameter<StringArray>)Parameters["PointNames"]; }
    }

    public DoubleMatrix DistanceMatrix {
      get {
        return DistanceMatrixParameter.Value;
      }
      set {
        DistanceMatrixParameter.Value = value;
      }
    }

    public IntMatrix PointMapping {
      get {
        return PointMappingParameter.Value;
      }
      set {
        PointMappingParameter.Value = value;
      }
    }

    public StringArray PointNames {
      get {
        return PointNamesParameter.Value;
      }
      set {
        PointNamesParameter.Value = value;
      }
    }

    public DistanceMatrixMeasure()
      : base() {
        Parameters.Add(new ValueParameter<DoubleMatrix>("DistanceMatrix", new DoubleMatrix()));
        Parameters.Add(new ValueParameter<IntMatrix>("PointMapping", new IntMatrix()));
        Parameters.Add(new OptionalValueParameter<StringArray>("PointNames"));

        DistanceMatrixParameter.GetsCollected = false;
        PointMappingParameter.GetsCollected = false;
        PointNamesParameter.GetsCollected = false;
    }
    [StorableConstructor]
    protected DistanceMatrixMeasure(bool deserializing) : base(deserializing) {
    }
    protected DistanceMatrixMeasure(DistanceMatrixMeasure original, Cloner cloner)
      : base(original, cloner) {
    }

    public override IDeepCloneable Clone(Cloner cloner) {
      return new DistanceMatrixMeasure(this, cloner);
    }

    public override bool DiversionSupported {
      get { return false; }
    }

    public int GetMapping(double x, double y) {
      IntMatrix pointMapping = PointMappingParameter.Value;
      int index = -1;

      int ix = (int)Math.Round(x);
      int iy = (int)Math.Round(y);

      int i = 0;
      while (index < 0 && i < pointMapping.Rows) {
        if (pointMapping[i, 0] == ix && pointMapping[i, 1] == iy) {
          index = i;
        }         
        i++;
      }

      return index;
    }

    protected double GetEuclideanDistance(double sourceX, double sourceY, double destX, double destY) {
      double vx = destX - sourceX;
      double vy = destY - sourceY;
      double length = Math.Sqrt(vx * vx + vy * vy);

      return length;
    }

    public override double GetDistance(double sourceX, double sourceY,
      double currentX, double currentY, 
      double destX, double destY) {
      double total = GetEuclideanDistance(sourceX, sourceY, destX, destY);
      double driven = GetEuclideanDistance(sourceX, sourceY, currentX, currentY);

      double completed = 1.0;
      if(total != 0)
        completed = driven / total;

      return GetDistance(sourceX, sourceY, destX, destY) * (1.0 - completed);
    }

    public override double GetDistance(double sourceX, double sourceY,
     double destX, double destY) {
      DoubleMatrix distanceMatrix = DistanceMatrixParameter.Value;

      int i = GetMapping(sourceX, sourceY);
      int j = GetMapping(destX, destY);

      return distanceMatrix[i, j];
    }

    public override void Move(double sourceX, double sourceY,
      double currentX, double currentY,
      double destX, double destY, double time, Vehicle.MoveInformation moveInfo,
      out double newPosX, out double newPosY, out double length) {
      
      length = GetDistance(sourceX, sourceY, currentX, currentY, destX, destY);
      if (length < time) {
        newPosX = destX;
        newPosY = destY;
      } else {
        double step = GetEuclideanDistance(sourceX, sourceY, destX, destY) /
          GetDistance(sourceX, sourceY, destX, destY);
        double vx = destX - currentX;
        double vy = destY - currentY;
        double vl = GetEuclideanDistance(currentX, currentY, destX, destY);
        vx = (vx / vl) * time * step;
        vy = (vy / vl) * time * step;

        newPosX = currentX + vx;
        newPosY = currentY + vy;
        length = time;
      }
    }
  }
}