//Copyright (c) 2010 Alexander Noeth
//
//Permission is hereby granted, free of charge, to any person obtaining a copy
//of this software and associated documentation files (the "Software"), to de$
//in the Software without restriction, including without limitation the rights
//to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//copies of the Software, and to permit persons to whom the Software is
//furnished to do so, subject to the following conditions:
//
//The above copyright notice and this permission notice shall be included in
//all copies or substantial portions of the Software.
//
//THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FRO$
//OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//THE SOFTWARE.
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;

/**
 * 46.861% reduction (judging+training)
 * 48.241% reduction (judging)
 * 
 * @author Alex Noeth
 */

public class OptimizedTargetPolicy implements Policy {
    private final static String name = "Target Policy AN";
    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
    public final static int HIST_PERIODS = 130;
    // value used for the initialization of the moving average
    private final static double MOTOR_POWER_HIST_INIT = -10000.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 = -20000.0;
    // battery target current 
    private final static double BATT_TARGET_CURRENT = -4900.0;

    
    public void set(double i){
	//System.out.println("!!!!!!!!!!!!!!!");
	//BATT_TARGET_CURRENT=i;
	//System.out.println(BATT_TARGET_CURRENT);
    }
    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.getMinPower(periodMS);
        final double capMaxCurrent = modelCap.getMaxPower(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(modelCap.getCharge()==modelCap.getMaxCharge()&&motorPower>0){
            System.out.print("D");
        }
        if(modelCap.getCharge()==0&&motorPower<30000){
            System.out.print("E");
        }
        */
        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;
                //battToCap=Math.max(Math.max(battTargetCurrent - motorPower, battToCapTarget), -capTargetCurrent)/8;
            } 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.drawPower(capToMotor - battToCap, pf);
            modelBatt.drawPower(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;
    }
}