from influxdb import InfluxDBClient
import time
import Adafruit_ADS1x15
import RPi.GPIO as GPIO
import random
import json
import datetime
import wiringpi

    #ustawienia bazy danych
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')


    #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


        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):
        error_value = self.error
        return error_value

    def getIntegrator(self):
        return self.integrator

    def getDerivator(self):
        return self.derivator


    #InfluxDB send JSON
def 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": int(round(pid_value,0))
            }
        },
        {
            "measurement": "P",
            "time": datetime.datetime.now(),
            "fields": {
                "value": p_value
            }
        },
        {
            "measurement": "I",
            "time": datetime.datetime.now(),
            "fields": {
                "value": i_value
            }
        },
        {
            "measurement": "D",
            "time": datetime.datetime.now(),
            "fields": {
                "value": d_value
            }
        },
        {
            "measurement": "PID",
            "time": datetime.datetime.now(),
            "fields": {
                "value": pid_value
            }
        }]

    client.write_points(json_body)




    #petla glowna
distance=600            # 600
p_value=1.0             # 1.0
i_value=0.11            # 0.11
d_value=2.5             # 2.5
p = PID(p_value,i_value,d_value) #nastawy PID
p.setPoint(distance)     #zadana odleglosc od czujnika


while True:

    for x in xrange(1,3):

        reszta = x % 2
        adc_value = get_adc_value()
        pid_value = p.update(adc_value)
        servo(int(round(pid_value,0)))
        if reszta == 1:
            grafana()
        time.sleep(0.17)