/*
 * Copyright (C) 2015 The Android Open Source Project
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

package com.android.car;

import android.car.hardware.CarSensorEvent;
import android.car.hardware.CarSensorManager;
import android.car.hardware.ICarSensorEventListener;
import android.content.Context;
import android.os.SystemClock;
import android.util.Log;

import java.io.PrintWriter;
import java.util.List;


/**
 * Logical sensor implementing driving state policy. This policy sets only two states:
 * no restriction vs fully restrictive. To enter no restriction state, speed should be zero
 * while either parking brake is applied or transmission gear is in P.
 */
public class DrivingStatePolicy extends CarSensorService.LogicalSensorHalBase {

    private final Context mContext;
    private CarSensorService mSensorService;
    private int mDrivingState = CarSensorEvent.DRIVE_STATUS_FULLY_RESTRICTED;
    private SensorListener mSensorListener;
    private boolean mIsReady = false;
    private boolean mStarted = false;

    private static final int[] SUPPORTED_SENSORS = { CarSensorManager.SENSOR_TYPE_DRIVING_STATUS };

    private final ICarSensorEventListener mICarSensorEventListener =
            new ICarSensorEventListener.Stub() {
        @Override
        public void onSensorChanged(List<CarSensorEvent> events) {
            for (CarSensorEvent event: events) {
                handleSensorEvent(event);
            }
        }
    };

    public DrivingStatePolicy(Context context, CarSensorService sensorService) {
        mContext = context;
        mSensorService = sensorService;
    }

    @Override
    public void init() {
        mIsReady = true;
    }

    @Override
    public synchronized void onSensorServiceReady() {
        int sensorList[] = mSensorService.getSupportedSensors();
        boolean hasSpeed = subscribeIfSupportedLocked(sensorList,
                CarSensorManager.SENSOR_TYPE_CAR_SPEED, CarSensorManager.SENSOR_RATE_FASTEST);
        if (!hasSpeed) {
            Log.w(CarLog.TAG_SENSOR,
                    "No speed sensor from car. Driving state will be always fully restrictive");
        }
        boolean hasParkingBrake = subscribeIfSupportedLocked(sensorList,
                CarSensorManager.SENSOR_TYPE_PARKING_BRAKE, CarSensorManager.SENSOR_RATE_FASTEST);
        boolean hasGear = subscribeIfSupportedLocked(sensorList, CarSensorManager.SENSOR_TYPE_GEAR,
                CarSensorManager.SENSOR_RATE_FASTEST);
        if (!hasParkingBrake && !hasGear) {
            Log.w(CarLog.TAG_SENSOR,
                    "No brake info from car. Driving state will be always fully restrictive");
        }
    }

    @Override
    public void release() {
    }

    public static CarSensorEvent getDefaultValue(int sensorType) {
        if (sensorType != CarSensorManager.SENSOR_TYPE_DRIVING_STATUS) {
            Log.w(CarLog.TAG_SENSOR, "getDefaultValue to DrivingStatePolicy with sensorType:" +
                    sensorType);
            return null;
        }
        // There's a race condition and timestamp from vehicle HAL could be slightly less
        // then current call to SystemClock.elapsedRealtimeNanos() will return.
        // We want vehicle HAL value always override this default value so we set timestamp to 0.
        return createEvent(CarSensorEvent.DRIVE_STATUS_FULLY_RESTRICTED, 0 /* timestamp */);
    }

    @Override
    public synchronized void registerSensorListener(SensorListener listener) {
        mSensorListener = listener;
    }

    @Override
    public synchronized boolean isReady() {
        return mIsReady;
    }

    @Override
    public int[] getSupportedSensors() {
        return SUPPORTED_SENSORS;
    }

    @Override
    public synchronized boolean requestSensorStart(int sensorType, int rate) {
        mStarted = true;
        dispatchCarSensorEvent(mSensorListener, createEvent(mDrivingState));
        return true;
    }

    @Override
    public synchronized void requestSensorStop(int sensorType) {
        mStarted = false;
    }

    @Override
    public void dump(PrintWriter writer) {
    }

    private boolean subscribeIfSupportedLocked(int sensorList[], int sensorType, int rate) {
        if (!CarSensorManager.isSensorSupported(sensorList, sensorType)) {
            Log.i(CarLog.TAG_SENSOR, "Sensor not supported:" + sensorType);
            return false;
        }
        return mSensorService.registerOrUpdateSensorListener(sensorType, rate,
                mICarSensorEventListener);
    }

    private synchronized void handleSensorEvent(CarSensorEvent event) {
        switch (event.sensorType) {
            case CarSensorManager.SENSOR_TYPE_PARKING_BRAKE:
            case CarSensorManager.SENSOR_TYPE_GEAR:
            case CarSensorManager.SENSOR_TYPE_CAR_SPEED:
                int drivingState = recalcDrivingStateLocked();
                if (drivingState != mDrivingState && mSensorListener != null) {
                    mDrivingState = drivingState;
                    dispatchCarSensorEvent(mSensorListener, createEvent(mDrivingState));
                }
                break;
            default:
                break;
        }
    }

    private int recalcDrivingStateLocked() {
        int drivingState = CarSensorEvent.DRIVE_STATUS_FULLY_RESTRICTED;
        CarSensorEvent lastParkingBrake = mSensorService.getLatestSensorEvent(
                CarSensorManager.SENSOR_TYPE_PARKING_BRAKE);
        CarSensorEvent lastGear = mSensorService.getLatestSensorEvent(
                CarSensorManager.SENSOR_TYPE_GEAR);
        CarSensorEvent lastSpeed = mSensorService.getLatestSensorEvent(
                CarSensorManager.SENSOR_TYPE_CAR_SPEED);
        if (lastSpeed != null && lastSpeed.floatValues[0] == 0f) { // stopped
            if (lastParkingBrake == null && isParkingBrakeApplied(lastParkingBrake)) {
                if (lastGear != null && isGearInParkingOrNeutral(lastGear)) {
                    drivingState = CarSensorEvent.DRIVE_STATUS_UNRESTRICTED;
                }
            } else { // parking break not applied or not available
                if (lastGear != null && isGearInParking(lastGear)) { // gear in P
                    drivingState = CarSensorEvent.DRIVE_STATUS_UNRESTRICTED;
                }
            }
        } // else moving, full restriction
        return drivingState;
    }

    private boolean isSpeedZero(CarSensorEvent event) {
        return event.floatValues[0] == 0f;
    }

    private boolean isParkingBrakeApplied(CarSensorEvent event) {
        return event.intValues[0] == 1;
    }

    private boolean isGearInParkingOrNeutral(CarSensorEvent event) {
        int gear = event.intValues[0];
        return (gear == CarSensorEvent.GEAR_NEUTRAL) ||
                (gear == CarSensorEvent.GEAR_PARK);
    }

    private boolean isGearInParking(CarSensorEvent event) {
        int gear = event.intValues[0];
        return gear == CarSensorEvent.GEAR_PARK;
    }

    private static CarSensorEvent createEvent(int drivingState) {
        return createEvent(drivingState, SystemClock.elapsedRealtimeNanos());
    }

    private static CarSensorEvent createEvent(int drivingState, long timestamp) {
        CarSensorEvent event = new CarSensorEvent(
                CarSensorManager.SENSOR_TYPE_DRIVING_STATUS,
                timestamp,
                0 /* float values */,
                1 /* int values */);
        event.intValues[0] = drivingState;
        return event;
    }
}