/**
 * The path-instance of the kinematic model
 *
 * @author ab
 */
package btools.router;

final class KinematicPath extends OsmPath {
  private double ekin; // kinetic energy (Joule)
  private double totalTime;  // travel time (seconds)
  private double totalEnergy; // total route energy (Joule)
  private float floatingAngleLeft; // sliding average left bend (degree)
  private float floatingAngleRight; // sliding average right bend (degree)

  @Override
  protected void init(OsmPath orig) {
    KinematicPath origin = (KinematicPath) orig;
    ekin = origin.ekin;
    totalTime = origin.totalTime;
    totalEnergy = origin.totalEnergy;
    floatingAngleLeft = origin.floatingAngleLeft;
    floatingAngleRight = origin.floatingAngleRight;
  }

  @Override
  protected void resetState() {
    ekin = 0.;
    totalTime = 0.;
    totalEnergy = 0.;
    floatingAngleLeft = 0.f;
    floatingAngleRight = 0.f;
  }

  @Override
  protected double processWaySection(RoutingContext rc, double dist, double delta_h, double elevation, double angle, double cosangle, boolean isStartpoint, int nsection, int lastpriorityclassifier) {
    KinematicModel km = (KinematicModel) rc.pm;

    double cost = 0.;
    double extraTime = 0.;

    if (isStartpoint) {
      // for forward direction, we start with target speed
      if (!rc.inverseDirection) {
        extraTime = 0.5 * (1. - cosangle) * 40.; // 40 seconds turn penalty
      }
    } else {
      double turnspeed = 999.; // just high

      if (km.turnAngleDecayTime != 0.) { // process turn-angle slowdown
        if (angle < 0) floatingAngleLeft -= (float) angle;
        else floatingAngleRight += (float) angle;
        float aa = Math.max(floatingAngleLeft, floatingAngleRight);

        double curveSpeed = aa > 10. ? 200. / aa : 20.;
        double distanceTime = dist / curveSpeed;
        double decayFactor = Math.exp(-distanceTime / km.turnAngleDecayTime);
        floatingAngleLeft = (float) (floatingAngleLeft * decayFactor);
        floatingAngleRight = (float) (floatingAngleRight * decayFactor);

        if (curveSpeed < 20.) {
          turnspeed = curveSpeed;
        }
      }

      if (nsection == 0) { // process slowdown by crossing geometry
        double junctionspeed = 999.; // just high

        int classifiermask = (int) rc.expctxWay.getClassifierMask();

        // penalty for equal priority crossing
        boolean hasLeftWay = false;
        boolean hasRightWay = false;
        boolean hasResidential = false;
        for (OsmPrePath prePath = rc.firstPrePath; prePath != null; prePath = prePath.next) {
          KinematicPrePath pp = (KinematicPrePath) prePath;

          if (((pp.classifiermask ^ classifiermask) & 8) != 0) { // exactly one is linktype
            continue;
          }

          if ((pp.classifiermask & 32) != 0) { // touching a residential?
            hasResidential = true;
          }

          if (pp.priorityclassifier > priorityclassifier || pp.priorityclassifier == priorityclassifier && priorityclassifier < 20) {
            double diff = pp.angle - angle;
            if (diff < -40. && diff > -140.) hasLeftWay = true;
            if (diff > 40. && diff < 140.) hasRightWay = true;
          }
        }
        double residentialSpeed = 13.;

        if (hasLeftWay && junctionspeed > km.leftWaySpeed) junctionspeed = km.leftWaySpeed;
        if (hasRightWay && junctionspeed > km.rightWaySpeed) junctionspeed = km.rightWaySpeed;
        if (hasResidential && junctionspeed > residentialSpeed) junctionspeed = residentialSpeed;

        if ((lastpriorityclassifier < 20) ^ (priorityclassifier < 20)) {
          extraTime += 10.;
          junctionspeed = 0; // full stop for entering or leaving road network
        }

        if (lastpriorityclassifier != priorityclassifier && (classifiermask & 8) != 0) {
          extraTime += 2.; // two seconds for entering a link-type
        }
        turnspeed = turnspeed > junctionspeed ? junctionspeed : turnspeed;

        if (message != null) {
          message.vnode0 = (int) (junctionspeed * 3.6 + 0.5);
        }
      }
      cutEkin(km.totalweight, turnspeed); // apply turnspeed
    }

    // linear temperature correction
    double tcorr = (20. - km.outside_temp) * 0.0035;

    // air_pressure down 1mb/8m
    double ecorr = 0.0001375 * (elevation - 100.);

    double f_air = km.f_air * (1. + tcorr - ecorr);

    double distanceCost = evolveDistance(km, dist, delta_h, f_air);

    if (message != null) {
      message.costfactor = (float) (distanceCost / dist);
      message.vmax = (int) (km.getWayMaxspeed() * 3.6 + 0.5);
      message.vmaxExplicit = (int) (km.getWayMaxspeedExplicit() * 3.6 + 0.5);
      message.vmin = (int) (km.getWayMinspeed() * 3.6 + 0.5);
      message.extraTime = (int) (extraTime * 1000);
    }

    cost += extraTime * km.pw / km.cost0;
    totalTime += extraTime;

    return cost + distanceCost;
  }


  protected double evolveDistance(KinematicModel km, double dist, double delta_h, double f_air) {
    // elevation force
    double fh = delta_h * km.totalweight * 9.81 / dist;

    double effectiveSpeedLimit = km.getEffectiveSpeedLimit();
    double emax = 0.5 * km.totalweight * effectiveSpeedLimit * effectiveSpeedLimit;
    if (emax <= 0.) {
      return -1.;
    }
    double vb = km.getBreakingSpeed(effectiveSpeedLimit);
    double elow = 0.5 * km.totalweight * vb * vb;

    double elapsedTime = 0.;
    double dissipatedEnergy = 0.;

    double v = Math.sqrt(2. * ekin / km.totalweight);
    double d = dist;
    while (d > 0.) {
      boolean slow = ekin < elow;
      boolean fast = ekin >= emax;
      double etarget = slow ? elow : emax;
      double f = km.f_roll + f_air * v * v + fh;
      double f_recup = Math.max(0., fast ? -f : (slow ? km.f_recup : 0) - fh); // additional recup for slow part
      f += f_recup;

      double delta_ekin;
      double timeStep;
      double x;
      if (fast) {
        x = d;
        delta_ekin = x * f;
        timeStep = x / v;
        ekin = etarget;
      } else {
        delta_ekin = etarget - ekin;
        double b = 2. * f_air / km.totalweight;
        double x0 = delta_ekin / f;
        double x0b = x0 * b;
        x = x0 * (1. - x0b * (0.5 + x0b * (0.333333333 - x0b * 0.25))); // = ln( delta_ekin*b/f + 1.) / b;
        double maxstep = Math.min(50., d);
        if (x >= maxstep) {
          x = maxstep;
          double xb = x * b;
          delta_ekin = x * f * (1. + xb * (0.5 + xb * (0.166666667 + xb * 0.0416666667))); // = f/b* exp(xb-1)
          ekin += delta_ekin;
        } else {
          ekin = etarget;
        }
        double v2 = Math.sqrt(2. * ekin / km.totalweight);
        double a = f / km.totalweight; // TODO: average force?
        timeStep = (v2 - v) / a;
        v = v2;
      }
      d -= x;
      elapsedTime += timeStep;

      // dissipated energy does not contain elevation and efficient recup
      dissipatedEnergy += delta_ekin - x * (fh + f_recup * km.recup_efficiency);

      // correction: inefficient recup going into heating is half efficient
      double ieRecup = x * f_recup * (1. - km.recup_efficiency);
      double eaux = timeStep * km.p_standby;
      dissipatedEnergy -= Math.max(ieRecup, eaux) * 0.5;
    }

    dissipatedEnergy += elapsedTime * km.p_standby;

    totalTime += elapsedTime;
    totalEnergy += dissipatedEnergy + dist * fh;

    return (km.pw * elapsedTime + dissipatedEnergy) / km.cost0; // =cost
  }

  @Override
  protected double processTargetNode(RoutingContext rc) {
    KinematicModel km = (KinematicModel) rc.pm;

    // finally add node-costs for target node
    if (targetNode.nodeDescription != null) {
      rc.expctxNode.evaluate(false, targetNode.nodeDescription);
      float initialcost = rc.expctxNode.getInitialcost();
      if (initialcost >= 1000000.) {
        return -1.;
      }
      cutEkin(km.totalweight, km.getNodeMaxspeed()); // apply node maxspeed

      if (message != null) {
        message.linknodecost += (int) initialcost;
        message.nodeKeyValues = rc.expctxNode.getKeyValueDescription(false, targetNode.nodeDescription);

        message.vnode1 = (int) (km.getNodeMaxspeed() * 3.6 + 0.5);
      }
      return initialcost;
    }
    return 0.;
  }

  private void cutEkin(double weight, double speed) {
    double e = 0.5 * weight * speed * speed;
    if (ekin > e) ekin = e;
  }


  @Override
  public int elevationCorrection() {
    return 0;
  }

  @Override
  public boolean definitlyWorseThan(OsmPath path) {
    KinematicPath p = (KinematicPath) path;

    int c = p.cost;
    return cost > c + 100;
  }

  @Override
  public double getTotalTime() {
    return totalTime;
  }

  @Override
  public double getTotalEnergy() {
    return totalEnergy;
  }
}
