package org.chargecar.prize.policies;

import java.util.ArrayDeque;
import java.util.Deque;

import org.chargecar.prize.battery.BatteryModel;
import org.chargecar.prize.util.PointFeatures;
import org.chargecar.prize.util.PowerFlowException;
import org.chargecar.prize.util.PowerFlows;
import org.chargecar.prize.util.TripFeatures;

/**
 * 28.886% reduction (judging+training)
 * 30.474% reduction (judging)
 * 
 * @author Jurgen van Dijk
 */

public class TargetPolicy implements Policy {
    private final static String name = "Target Policy";
    private final static double SECONDS_PER_HOUR = 3600.0;
    private BatteryModel modelCap;
    private BatteryModel modelBatt;
    private double capMaxCharge;
    private final Deque<Double> motorPowerHist = new ArrayDeque<Double>();
    private final Deque<Double> periodHist = new ArrayDeque<Double>();
    private double motorPowerHistSum;
    private double periodHistSum;
    // number of periods for calculating the moving average
    private final static int HIST_PERIODS = 60;
    // value used for the initialisation of the moving average
    private final static double MOTOR_POWER_HIST_INIT = -18000.0;
    // speed factor used to calculate the cap target charge
    private final static double SPEED_FACTOR = 0.558;
    // minimum current used for the battery to cap flow
    private final static double BATT_TO_CAP_MIN_CURRENT = -28000.0;
    // battery target current 
    private final static double BATT_TARGET_CURRENT = -4260.0;

    public void beginTrip(TripFeatures tripFeatures, BatteryModel batteryClone, BatteryModel capacitorClone) {
        modelCap = capacitorClone;
        modelBatt = batteryClone;

        capMaxCharge = modelCap.getMaxCharge();

        for (int i = 0; i < HIST_PERIODS; i++) {
            motorPowerHist.addLast(MOTOR_POWER_HIST_INIT);
            periodHist.addLast(1.0);
        }
        motorPowerHistSum = MOTOR_POWER_HIST_INIT * HIST_PERIODS;
        periodHistSum = 1.0 * HIST_PERIODS;
    }

    public PowerFlows calculatePowerFlows(PointFeatures pf) {
        final int periodMS = pf.getPeriodMS(); // period in milliseconds
        final double period = periodMS / 1000.0; // period in seconds
        final double speed = pf.getSpeed(); // speed in meters per second
        final double motorPower = pf.getPowerDemand(); // demanded or regenerated power in watts

        final double capCharge = modelCap.getCharge();
        final double capMinCurrent = modelCap.getMinCurrent(periodMS);
        final double capMaxCurrent = modelCap.getMaxCurrent(periodMS);

        motorPowerHistSum -= motorPowerHist.removeFirst();
        periodHistSum -= periodHist.removeFirst();
        motorPowerHistSum += motorPower;
        periodHistSum += period;
        motorPowerHist.addLast(motorPower);
        periodHist.addLast(period);
        final double motorMovAvg = motorPowerHistSum / periodHistSum;

        // 0 <= capTargetCharge <= capMaxCharge
        final double capTargetCharge = Math.max(capMaxCharge - speed * SPEED_FACTOR, 0.0);
        // 0 <= capTargetCurrent <= capMaxCurrent
        final double capTargetCurrent = Math.min(Math.max(capTargetCharge - capCharge, 0.0) * SECONDS_PER_HOUR, capMaxCurrent);
        // BATT_TO_CAP_MIN_CURRENT <= battToCapTarget <= 0
        final double battToCapTarget = (capTargetCharge < 1E-6 ? 0.0 : Math.max(1.0 - capCharge / capTargetCharge, 0.0)) * BATT_TO_CAP_MIN_CURRENT;
        // battTargetCurrent = min(battToCapTarget, motorMovAvg, BATT_TARGET_CURRENT) <= 0
        final double battTargetCurrent = Math.min(Math.min(battToCapTarget, motorMovAvg), BATT_TARGET_CURRENT);

        double battToMotor = 0.0;
        double capToMotor = 0.0;
        double battToCap = 0.0;

        if (motorPower < 0.0) {
            // motor is demanding power
            if (motorPower < battTargetCurrent) {
                // motor is demanding more power than battery target current
                // cap powers motor
                capToMotor = Math.max(motorPower - battTargetCurrent, capMinCurrent);
                // battery powers motor
                battToMotor = motorPower - capToMotor;
            } else if (motorPower > battTargetCurrent) {
                // motor is demanding less power than battery target current
                // battery powers motor
                battToMotor = motorPower;
                // battery charges cap
                battToCap = Math.max(Math.max(battTargetCurrent - motorPower, battToCapTarget), -capTargetCurrent);
            } else {
                // motor is exactly demanding battery target current
                // battery powers motor
                battToMotor = motorPower;
            }
        } else if (motorPower > 0.0) {
            // motor is generating power
            if (motorPower > capTargetCurrent) {
                // motor is generating more power than cap target current
                // motor charges cap
                capToMotor = Math.min(motorPower, capMaxCurrent);
            } else if (motorPower < capTargetCurrent) {
                // motor is generating less power than cap target current
                // motor charges cap
                capToMotor = motorPower;
                // battery charges cap
                battToCap = Math.max(motorPower - capTargetCurrent, battTargetCurrent);
            } else {
                // motor is exactly generating cap target current
                // motor charges cap
                capToMotor = motorPower;
            }
        }

        try {
            modelCap.drawCurrent(capToMotor - battToCap, pf);
            modelBatt.drawCurrent(battToMotor + battToCap, pf);
        } catch (PowerFlowException e) {
            System.err.println(e);
        }

        return new PowerFlows(battToMotor, capToMotor, battToCap);
    }

    public void endTrip() {
        modelCap = null;
        modelBatt = null;
        motorPowerHist.clear();
        periodHist.clear();
    }

    public void loadState() {
        // nothing to do
    }

    public String getName() {
        return name;
    }
}