package org.eclipse.leshan.client.demo;

import java.math.BigDecimal;
import java.math.RoundingMode;
import java.util.concurrent.Executors;
import java.util.concurrent.ScheduledExecutorService;
import java.util.concurrent.TimeUnit;


import org.eclipse.leshan.client.resource.BaseInstanceEnabler;
import org.eclipse.leshan.core.response.ExecuteResponse;
import org.eclipse.leshan.core.response.ReadResponse;
import org.eclipse.leshan.util.NamedThreadFactory;

import com.pi4j.io.gpio.GpioController;
import com.pi4j.io.gpio.GpioFactory;
import com.pi4j.io.gpio.GpioPinDigitalInput;
import com.pi4j.io.gpio.GpioPinDigitalOutput;
import com.pi4j.io.gpio.PinPullResistance;
import com.pi4j.io.gpio.RaspiPin;


public class DistanceSensor extends BaseInstanceEnabler {

	private static GpioPinDigitalOutput sensorTriggerPin ;
	private static GpioPinDigitalInput sensorEchoPin ;

	final static GpioController gpio = GpioFactory.getInstance();


	private static final String UNIT_CENTIMETER = "cm";
    private static final int SENSOR_VALUE = 5700;
    private static final int UNITS = 5701;
    private static final int MAX_MEASURED_VALUE = 5602;
    private static final int MIN_MEASURED_VALUE = 5601;
    private static final int RESET_MIN_MAX_MEASURED_VALUES = 5605;
    private final ScheduledExecutorService scheduler;

    private double currentDistance = 0d;
    private double minMeasuredValue = currentDistance;
    private double maxMeasuredValue = currentDistance;


    public DistanceSensor( ) {

    	this.scheduler = Executors.newSingleThreadScheduledExecutor(new NamedThreadFactory("Temperature Sensor"));
        scheduler.scheduleAtFixedRate(new Runnable() {

            @Override
            public void run() {
                adjustDistance();
            }
        }, 2, 2, TimeUnit.SECONDS);



    }

    public synchronized ReadResponse read(int resourceId) {
        switch (resourceId) {
        case MIN_MEASURED_VALUE:
            return ReadResponse.success(resourceId, getTwoDigitValue(minMeasuredValue));
        case MAX_MEASURED_VALUE:
            return ReadResponse.success(resourceId, getTwoDigitValue(maxMeasuredValue));
        case SENSOR_VALUE:
            return ReadResponse.success(resourceId, getTwoDigitValue(currentDistance));
        case UNITS:
            return ReadResponse.success(resourceId, UNIT_CENTIMETER);
        default:
            return super.read(resourceId);
        }
    }

    @Override
    public synchronized ExecuteResponse execute(int resourceId, String params) {
        switch (resourceId) {
        case RESET_MIN_MAX_MEASURED_VALUES:
            resetMinMaxMeasuredValues();
            return ExecuteResponse.success();
        default:
            return super.execute(resourceId, params);
        }
    }

    private double getTwoDigitValue(double value) {
        BigDecimal toBeTruncated = BigDecimal.valueOf(value);
        return toBeTruncated.setScale(2, RoundingMode.HALF_UP).doubleValue();
    }


    private synchronized void adjustDistance() {

    	sensorTriggerPin =  gpio.provisionDigitalOutputPin(RaspiPin.GPIO_04); // Trigger pin as OUTPUT
		sensorEchoPin = gpio.provisionDigitalInputPin(RaspiPin.GPIO_05,PinPullResistance.PULL_DOWN); // Echo pin as INPUT


			try {
			Thread.sleep(2000);
			sensorTriggerPin.high(); // Make trigger pin HIGH
			Thread.sleep((long) 0.00001);// Delay for 10 microseconds
			sensorTriggerPin.low(); //Make trigger pin LOW

			while(sensorEchoPin.isLow()){ //Wait until the ECHO pin gets HIGH

			}
			long startTime= System.nanoTime(); // Store the surrent time to calculate ECHO pin HIGH time.
			while(sensorEchoPin.isHigh()){ //Wait until the ECHO pin gets LOW

			}
			long endTime= System.nanoTime(); // Store the echo pin HIGH end time to calculate ECHO pin HIGH time.

			currentDistance = ((((endTime - startTime)/1e3)/2) / 29.1);
			Integer changedResource = adjustMinMaxMeasuredValue(currentDistance);
	        if (changedResource != null) {
	            fireResourcesChange(SENSOR_VALUE, changedResource);
	        } else {
	            fireResourcesChange(SENSOR_VALUE);
	        }
			Thread.sleep(1000);

			}
			catch (InterruptedException e) {
				e.printStackTrace();
			}




    }

    private Integer adjustMinMaxMeasuredValue(double newDistance) {

        if (newDistance > maxMeasuredValue) {
            maxMeasuredValue = newDistance;
            return MAX_MEASURED_VALUE;
        } else if (newDistance < minMeasuredValue) {
            minMeasuredValue = newDistance;
            return MIN_MEASURED_VALUE;
        } else {
            return null;
        }
    }

    private void resetMinMaxMeasuredValues() {
        minMeasuredValue = currentDistance;
        maxMeasuredValue = currentDistance;
    }


}