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


from luma.core.interface.serial import i2c, spi
from luma.core.render import canvas
from luma.oled.device import sh1106

    #ustawienia wyswietlacza OLED
serial=spi(device=0, port=0)
device=sh1106(serial, rotate=2)

    #ustawienia bazy danych
client = InfluxDBClient(host='104.248.129.33', port=8186)
# 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)






    #pomiar odleglosci kuli
def get_adc_value():

    adc_value = adc.get_last_result()
    adc_dist_value = a*math.exp(b*adc_value)
    #print('Channel 0: {0}'.format(adc_dist_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__(my, P=2.0, I=0.0, D=1.0, derivator=0, integrator=0, integrator_max=300, integrator_min=-300):

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

        my.set_point = 0.0
        my.error = 0.0

    def update(my, current_value):

        my.error = my.set_point - current_value

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

        my.integrator = my.integrator + my.error

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

        my.I_value = my.integrator * my.Ki

        pid_value = my.P_value + my.I_value + my.D_value


        return pid_value

    def setPoint(my, set_point):

        my.set_point = set_point
        my.integrator = 0
        my.derivator = 0




    #InfluxDB send JSON
def influx():


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

        },
        {
            "measurement": "wartosc_zadama",
            "time": datetime.datetime.now(),
            "fields": {
                "value": oled_distance
            }
        },
        {
            "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)


def oled_display():

    with canvas(device) as draw:

        draw.text((0,0), '  UKLAD KULA-BELKA', fill="white")
        draw.text((0,10), 'Wartosci', fill="white")
        draw.text((0,18), 'zadane:', fill="white")
        draw.text((0,28), 'Odl: {0}'.format(oled_distance), fill="white")
        draw.text((0,36), 'P : {0}'.format(p_value), fill="white")
        draw.text((0,44), 'I : {0}'.format(i_value), fill="white")
        draw.text((0,52), 'D : {0}'.format(d_value), fill="white")


        draw.text((64,10), 'Wartosci', fill="white")
        draw.text((64,18), 'odczytane:', fill="white")
        draw.text((64,28), 'Odl: {0}'.format(int(round(adc_distance))), fill="white")
        draw.text((64,36), 'PID: {0}'.format(pid_value), fill="white")



    #petla glowna

a=79.2731944
b=-0.002241179049
distance=600                        # 600
oled_distance=int(round(a*math.exp(b*distance),2))+8
p_value=1.0                         # 1.0
i_value=0.0                         # 0.11
d_value=0.0                         # 2.5
p = PID(p_value,i_value,d_value)    #nastawy PID
p.setPoint(distance)                #zadana odleglosc od czujnika


while True:
    adc_value = get_adc_value()
    adc_distance = a*math.exp(b*adc_value)
    pid_value = p.update(adc_value) #adc_distance
    pid_serwo = int(round(pid_value,0))
    servo(pid_serwo)
    oled_display()
    influx()
    print('Nastawy: ' + 'P: {0}'.format(p_value) + ' I: {0}'.format(i_value) + ' D: {0}'.format(d_value) + '   Wartosc zadana: {0}'.format(oled_distance) + '   Wartosc zmierzona: {0}'.format(round(adc_distance,2)))
    time.sleep(0.17)