from influxdb import InfluxDBClient
import time
import Adafruit_ADS1x15
import wiringpi
import random
import json
import datetime


	#ustawienia PWM
wiringpi.wiringPiSetupGpio()
wiringpi.pinMode(18, wiringpi.GPIO.PWM_OUTPUT)
wiringpi.pwmSetMode(wiringpi.GPIO.PWM_MODE_MS)

wiringpi.pwmSetClock(192)
wiringpi.pwmSetRange(2000)
delay_period = 0.01


	# ustawienia ADC
adc = Adafruit_ADS1x15.ADS1015()
GAIN = 1

adc.start_adc_comparator(0,  # Channel number
                         20000, 5000,  # High threshold value, low threshold value
                         active_low=True, traditional=True, latching=False,
                         num_readings=1, gain=GAIN)


	#InfluxDB



	#pomiar odleglosci kuli
def get_adc_value():

	adc_value = adc.get_last_result()
 	print('Channel 0: {0}'.format(adc_value))
	return adc_value



	#sterowanie serwem
def servo(serwo_value):
	if serwo_value > 208:
		serwo_value = 208
	if serwo_value < 98:
		serwo_value = 98

	wiringpi.pwmWrite(18, serwo_value)
	print('Serwo value: {0}'.format(serwo_value))


	#regulator PID
class PID:
	"""
	Discrete PID control
	"""

	def __init__(self, P=2.0, I=0.0, D=1.0, derivator=0, integrator=0, integrator_max=300, integrator_min=-300):

		self.Kp = P
		self.Ki = I
		self.Kd = D
		self.derivator = derivator
		self.integrator = integrator
		self.integrator_max = integrator_max
		self.integrator_min = integrator_min

		self.set_point = 0.0
		self.error = 0.0

	def update(self, current_value):
		"""
		Caldulate PID output value for given reference input and feedback
		"""

		self.error = self.set_point - current_value

		self.P_value = self.Kp * self.error
		self.D_value = self.Kd * (self.error - self.derivator)
		self.derivator = self.error

		self.integrator = self.integrator + self.error

		if self.integrator > self.integrator_max:
			self.integrator = self.integrator_max
		elif self.integrator < self.integrator_min:
			self.integrator = self.integrator_min

		self.I_value = self.integrator * self.Ki

		pid_value = self.P_value + self.I_value + self.D_value
		#pid_value = self.P_value + self.D_value

		return pid_value

	def setPoint(self, set_point):
		"""
		Initialize the setpoint od PID
		"""

		self.set_point = set_point
		self.integrator = 0
		self.derivator = 0

	def setIntegrator(self, integrator):
		self.integrator = integrator

	def setKp(self, P):
		self.Kp = P

	def setKi(self, I):
		self.Ki = I

	def setKi(self, D):
		self.Kd = D

	def getPoint(self):
		return self.set_point

	def getError(self):
		return self.error

	def getIntegrator(self):
		return self.integrator

	def getDerivator(self):
		return self.derivator


	#InfluxDB send JSON
def grafana():
	client = InfluxDBClient(host='localhost', port=8086)
	client = InfluxDBClient(host='localhost', port=8086, username='myuser', password='mypass', ssl=False, verify_ssl=False)
	client.create_database('grafana')
	client.switch_database('grafana')

	json_body = [
		{
			"measurement": "odleglosc",
			"time": datetime.datetime.now(),
			"fields": {
				"value": adc_value
			}

		},
		{
			"measurement": "wartosc_zadama",
			"time": datetime.datetime.now(),
			"fields": {
				"value": distance
			}
		},
		{
			"measurement": "pid",
			"time": datetime.datetime.now(),
			"fields": {
				"value": pid_value
			}
		}]

	client.write_points(json_body)




	#petla glowna
distance=550
p = PID(1.0,0.3,3.5) #nastawy PID
p.setPoint(distance)	 #zadana odleglosc od czujnika


while True:

	adc_value = get_adc_value()
	pid_value = p.update(adc_value)
	print("wartosc pid_value: ")
	print(int(round(pid_value,0)))
	servo(int(round(pid_value,0)))
	grafana()
	time.sleep(0.3)