package org.opentrafficsim.demo.steering;

import java.util.ArrayList;
import java.util.List;

import org.djunits.unit.FrequencyUnit;
import org.djunits.unit.MassUnit;
import org.djunits.unit.SpeedUnit;
import org.djunits.unit.TimeUnit;
import org.djunits.value.ValueRuntimeException;
import org.djunits.value.storage.StorageType;
import org.djunits.value.vdouble.scalar.Direction;
import org.djunits.value.vdouble.scalar.Length;
import org.djunits.value.vdouble.scalar.Mass;
import org.djunits.value.vdouble.scalar.Speed;
import org.djunits.value.vdouble.vector.FrequencyVector;
import org.djunits.value.vdouble.vector.TimeVector;
import org.djunits.value.vdouble.vector.base.DoubleVector;
import org.djutils.cli.CliUtil;
import org.opentrafficsim.base.parameters.ParameterException;
import org.opentrafficsim.base.parameters.ParameterSet;
import org.opentrafficsim.base.parameters.Parameters;
import org.opentrafficsim.core.compatibility.Compatible;
import org.opentrafficsim.core.dsol.OTSSimulatorInterface;
import org.opentrafficsim.core.geometry.Bezier;
import org.opentrafficsim.core.geometry.OTSLine3D;
import org.opentrafficsim.core.geometry.OTSPoint3D;
import org.opentrafficsim.core.gtu.GTUException;
import org.opentrafficsim.core.gtu.GTUType;
import org.opentrafficsim.core.network.LinkType;
import org.opentrafficsim.core.network.Node;
import org.opentrafficsim.core.network.OTSNode;
import org.opentrafficsim.core.units.distributions.ContinuousDistMass;
import org.opentrafficsim.road.gtu.generator.od.DefaultGTUCharacteristicsGeneratorOD;
import org.opentrafficsim.road.gtu.generator.od.ODApplier;
import org.opentrafficsim.road.gtu.generator.od.ODOptions;
import org.opentrafficsim.road.gtu.generator.od.StrategicalPlannerFactorySupplierOD;
import org.opentrafficsim.road.gtu.generator.od.StrategicalPlannerFactorySupplierOD.TacticalPlannerFactorySupplierOD;
import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
import org.opentrafficsim.road.gtu.lane.VehicleModel;
import org.opentrafficsim.road.gtu.lane.VehicleModelFactory;
import org.opentrafficsim.road.gtu.lane.tactical.AbstractLaneBasedTacticalPlannerFactory;
import org.opentrafficsim.road.gtu.lane.tactical.LaneBasedTacticalPlannerFactory;
import org.opentrafficsim.road.gtu.lane.tactical.following.IDMPlusFactory;
import org.opentrafficsim.road.gtu.lane.tactical.lmrs.DefaultLMRSPerceptionFactory;
import org.opentrafficsim.road.gtu.lane.tactical.steering.SteeringLmrs;
import org.opentrafficsim.road.gtu.lane.tactical.util.Steering;
import org.opentrafficsim.road.gtu.lane.tactical.util.Steering.FeedbackTable;
import org.opentrafficsim.road.gtu.lane.tactical.util.Steering.FeedbackTable.FeedbackVector;
import org.opentrafficsim.road.gtu.lane.tactical.util.lmrs.Cooperation;
import org.opentrafficsim.road.gtu.lane.tactical.util.lmrs.GapAcceptance;
import org.opentrafficsim.road.gtu.lane.tactical.util.lmrs.Synchronization;
import org.opentrafficsim.road.gtu.strategical.od.Categorization;
import org.opentrafficsim.road.gtu.strategical.od.Category;
import org.opentrafficsim.road.gtu.strategical.od.Interpolation;
import org.opentrafficsim.road.gtu.strategical.od.ODMatrix;
import org.opentrafficsim.road.network.OTSRoadNetwork;
import org.opentrafficsim.road.network.lane.CrossSectionLink;
import org.opentrafficsim.road.network.lane.Lane;
import org.opentrafficsim.road.network.lane.LaneType;
import org.opentrafficsim.road.network.lane.OTSRoadNode;
import org.opentrafficsim.road.network.lane.Stripe;
import org.opentrafficsim.road.network.lane.Stripe.Permeable;
import org.opentrafficsim.road.network.lane.changing.LaneKeepingPolicy;
import org.opentrafficsim.road.network.lane.object.sensor.Detector;
import org.opentrafficsim.road.network.lane.object.sensor.SinkSensor;
import org.opentrafficsim.swing.script.AbstractSimulationScript;

import nl.tudelft.simulation.jstats.distributions.DistUniform;
import nl.tudelft.simulation.jstats.streams.StreamInterface;
import picocli.CommandLine.Option;

/**
 * Simulation script for steering functionality.
 * <p>
 * Copyright (c) 2013-2020 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
 * BSD-style license. See <a href="http://opentrafficsim.org/node/13">OpenTrafficSim License</a>.
 * <p>
 * @version $Revision$, $LastChangedDate$, by $Author$, initial version 8 jan. 2019 <br>
 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
 * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
 * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
 */
public class SteeringSimulation extends AbstractSimulationScript
{

    /** Feedback table. */
    static final FeedbackTable FEEDBACK_CAR;

    /** Number of lanes. */
    @Option(names = "--numberOfLanes", description = "Number of lanes", defaultValue = "2")
    private int numberOfLanes;

    static
    {
        // TODO: define tables
        List<FeedbackVector> list = new ArrayList<>();
        list.add(new FeedbackVector(new Speed(25.0, SpeedUnit.KM_PER_HOUR), 0.0, 0.0, 0.0, 0.0));
        list.add(new FeedbackVector(new Speed(75.0, SpeedUnit.KM_PER_HOUR), 0.0, 0.0, 0.0, 0.0));
        FEEDBACK_CAR = new FeedbackTable(list);
    }

    /**
     * Start a simulation.
     * @param args String...; command line arguments
     */
    public static void main(final String... args)
    {
        try
        {
            SteeringSimulation sim = new SteeringSimulation();
            CliUtil.execute(sim, args);
            sim.start();
        }
        catch (Exception ex)
        {
            ex.printStackTrace();
        }
    }

    /**
     * Constructor.
     */
    protected SteeringSimulation()
    {
        super("Steering simulation", "Steering simulation");
    }

    /**
     * Sets up the simulation based on provided properties. Properties can be obtained with {@code getProperty()}. Setting up a
     * simulation should at least create a network and some demand. Additionally this may setup traffic control, sampling, etc.
     * @param sim OTSSimulatorInterface; simulator
     * @return OTSRoadNetwork; network
     * @throws Exception on any exception
     */
    @Override
    protected OTSRoadNetwork setupSimulation(final OTSSimulatorInterface sim) throws Exception
    {
        OTSRoadNetwork network = new OTSRoadNetwork("Steering network", true, getSimulator());
        Length laneWidth = Length.instantiateSI(3.5);
        Length stripeWidth = Length.instantiateSI(0.2);

        // points
        OTSPoint3D pointA = new OTSPoint3D(0, 0);
        OTSPoint3D pointB = new OTSPoint3D(2000, 0);
        OTSPoint3D pointC = new OTSPoint3D(2250, 0);
        OTSPoint3D pointD = new OTSPoint3D(3250, 0);
        OTSPoint3D pointE = new OTSPoint3D(1500, -30);

        // nodes
        OTSRoadNode nodeA = new OTSRoadNode(network, "A", pointA, Direction.ZERO);
        OTSRoadNode nodeB = new OTSRoadNode(network, "B", pointB, Direction.ZERO);
        OTSRoadNode nodeC = new OTSRoadNode(network, "C", pointC, Direction.ZERO);
        OTSRoadNode nodeD = new OTSRoadNode(network, "D", pointD, Direction.ZERO);
        OTSRoadNode nodeE = new OTSRoadNode(network, "E", pointE, Direction.ZERO);

        // links
        CrossSectionLink linkAB =
                new CrossSectionLink(network, "AB", nodeA, nodeB, network.getLinkType(LinkType.DEFAULTS.FREEWAY),
                        new OTSLine3D(pointA, pointB), LaneKeepingPolicy.KEEPRIGHT);
        CrossSectionLink linkBC =
                new CrossSectionLink(network, "BC", nodeB, nodeC, network.getLinkType(LinkType.DEFAULTS.FREEWAY),
                        new OTSLine3D(pointB, pointC), LaneKeepingPolicy.KEEPRIGHT);
        CrossSectionLink linkCD =
                new CrossSectionLink(network, "CD", nodeC, nodeD, network.getLinkType(LinkType.DEFAULTS.FREEWAY),
                        new OTSLine3D(pointC, pointD), LaneKeepingPolicy.KEEPRIGHT);
        CrossSectionLink linkEB =
                new CrossSectionLink(network, "EB", nodeE, nodeB, network.getLinkType(LinkType.DEFAULTS.FREEWAY),
                        Bezier.cubic(nodeE.getLocation(), nodeB.getLocation()), LaneKeepingPolicy.KEEPRIGHT);

        // lanes and stripes
        List<Lane> originLanes = new ArrayList<>();
        for (int i = 0; i < this.numberOfLanes; i++)
        {
            for (CrossSectionLink link : new CrossSectionLink[] {linkAB, linkBC, linkCD})
            {
                Lane lane = new Lane(link, "Lane " + (i + 1), laneWidth.times((0.5 + i)), laneWidth,
                        network.getLaneType(LaneType.DEFAULTS.FREEWAY), new Speed(120, SpeedUnit.KM_PER_HOUR));
                Length offset = laneWidth.times(i + 1.0);
                Stripe stripe = new Stripe(link, offset, offset, stripeWidth);
                if (i < this.numberOfLanes - 1)
                {
                    stripe.addPermeability(network.getGtuType(GTUType.DEFAULTS.VEHICLE), Permeable.BOTH);
                }
                // sink sensors
                if (lane.getParentLink().getId().equals("CD"))
                {
                    new SinkSensor(lane, lane.getLength().minus(Length.instantiateSI(100.0)), Compatible.EVERYTHING, sim);
                    // detectors 100m after on ramp
                    new Detector(lane.getFullId(), lane, Length.instantiateSI(100.0), sim); // id equal to lane, may be
                                                                                            // different
                }
                if (lane.getParentLink().getId().equals("AB"))
                {
                    originLanes.add(lane);
                }
            }
        }
        new Stripe(linkAB, Length.ZERO, Length.ZERO, stripeWidth);
        Stripe stripe = new Stripe(linkBC, Length.ZERO, Length.ZERO, stripeWidth);
        stripe.addPermeability(network.getGtuType(GTUType.DEFAULTS.VEHICLE), Permeable.LEFT);
        new Stripe(linkCD, Length.ZERO, Length.ZERO, stripeWidth);
        new Lane(linkBC, "Acceleration lane", laneWidth.times(-0.5), laneWidth, network.getLaneType(LaneType.DEFAULTS.FREEWAY),
                new Speed(120, SpeedUnit.KM_PER_HOUR));
        new Lane(linkEB, "Onramp", laneWidth.times(-0.5), laneWidth, network.getLaneType(LaneType.DEFAULTS.FREEWAY),
                new Speed(120, SpeedUnit.KM_PER_HOUR));
        new Stripe(linkEB, Length.ZERO, Length.ZERO, stripeWidth);
        new Stripe(linkEB, laneWidth.neg(), laneWidth.neg(), stripeWidth);
        new Stripe(linkBC, laneWidth.neg(), laneWidth.neg(), stripeWidth);

        // OD
        List<OTSNode> origins = new ArrayList<>();
        origins.add(nodeA);
        origins.add(nodeE);
        List<OTSNode> destinations = new ArrayList<>();
        destinations.add(nodeD);
        TimeVector timeVector = DoubleVector.instantiate(new double[] {0.0, 0.5, 1.0}, TimeUnit.BASE_HOUR, StorageType.DENSE);
        Interpolation interpolation = Interpolation.LINEAR; // or STEPWISE
        Categorization categorization = new Categorization("GTU type", GTUType.class);
        Category carCategory = new Category(categorization, network.getGtuType(GTUType.DEFAULTS.CAR));
        Category truCategory = new Category(categorization, network.getGtuType(GTUType.DEFAULTS.TRUCK));
        ODMatrix odMatrix = new ODMatrix("Steering OD", origins, destinations, categorization, timeVector, interpolation);

        odMatrix.putDemandVector(nodeA, nodeD, carCategory, freq(new double[] {1000.0, 2000.0, 0.0}));
        odMatrix.putDemandVector(nodeA, nodeD, truCategory, freq(new double[] {100.0, 200.0, 0.0}));
        odMatrix.putDemandVector(nodeE, nodeD, carCategory, freq(new double[] {500.0, 1000.0, 0.0}));

        // anonymous tactical-planner-factory supplier
        AbstractLaneBasedTacticalPlannerFactory<SteeringLmrs> car = new AbstractLaneBasedTacticalPlannerFactory<SteeringLmrs>(
                new IDMPlusFactory(sim.getReplication().getStream("generation")), new DefaultLMRSPerceptionFactory())
        {
            @Override
            public SteeringLmrs create(final LaneBasedGTU gtu) throws GTUException
            {
                return new SteeringLmrs(nextCarFollowingModel(gtu), gtu, getPerceptionFactory().generatePerception(gtu),
                        Synchronization.PASSIVE, Cooperation.PASSIVE, GapAcceptance.INFORMED, FEEDBACK_CAR);
            }

            @Override
            public Parameters getParameters() throws ParameterException
            {
                // TODO: add parameters if required (run and wait for ParameterException to find missing parameters)
                ParameterSet parameters = new ParameterSet();
                getCarFollowingParameters().setAllIn(parameters);
                parameters.setDefaultParameters(Steering.class);
                return parameters;
            }
        };
        TacticalPlannerFactorySupplierOD tacticalPlannerFactorySupplierOD = new TacticalPlannerFactorySupplierOD()
        {
            @Override
            public LaneBasedTacticalPlannerFactory<SteeringLmrs> getFactory(final Node origin, final Node destination,
                    final Category category, final StreamInterface randomStream)
            {
                GTUType gtuType = category.get(GTUType.class);
                if (gtuType.equals(network.getGtuType(GTUType.DEFAULTS.CAR)))
                {
                    return car;
                }
                else
                {
                    // TODO: other GTU types
                    return null;
                }
            };
        };
        // anonymous vehicle model factory
        // TODO: supply mass and inertia values, possibly randomized, correlated?
        ContinuousDistMass massDistCar =
                new ContinuousDistMass(new DistUniform(sim.getReplication().getStream("generation"), 600, 1200), MassUnit.SI);
        ContinuousDistMass massDistTruck =
                new ContinuousDistMass(new DistUniform(sim.getReplication().getStream("generation"), 2000, 10000), MassUnit.SI);
        double momentOfInertiaAboutZ = 100; // no idea...
        VehicleModelFactory vehicleModelGenerator = new VehicleModelFactory()
        {
            @Override
            public VehicleModel create(final GTUType gtuType)
            {
                Mass mass =
                        gtuType.isOfType(network.getGtuType(GTUType.DEFAULTS.CAR)) ? massDistCar.draw() : massDistTruck.draw();
                return new VehicleModel.MassBased(mass, momentOfInertiaAboutZ);
            }
        };
        // characteristics generator using OD info and default route based strategical level
        DefaultGTUCharacteristicsGeneratorOD characteristicsGenerator = new DefaultGTUCharacteristicsGeneratorOD.Factory()
                .setFactorySupplier(StrategicalPlannerFactorySupplierOD.route(tacticalPlannerFactorySupplierOD))
                .setVehicleModelGenerator(vehicleModelGenerator).create();

        // od options
        ODOptions odOptions = new ODOptions().set(ODOptions.NO_LC_DIST, Length.instantiateSI(300.0)).set(ODOptions.GTU_TYPE,
                characteristicsGenerator);
        ODApplier.applyOD(network, odMatrix, odOptions);

        return network;
    }

    /**
     * Creates a frequency vector.
     * @param array double[]; array in veh/h
     * @return FrequencyVector; frequency vector
     * @throws ValueRuntimeException on problem
     */
    private FrequencyVector freq(final double[] array) throws ValueRuntimeException
    {
        return DoubleVector.instantiate(array, FrequencyUnit.PER_HOUR, StorageType.DENSE);
    }

}