"""
Program     : Percobaan Kontroler NN menggunakan Micro Python ESP32
DiBuat Oleh : EruP-RMK
Keluaran    : v1.00
Tanggal     : 01 Juli 2005 - NN Versi MCS52
              05 Desember 2008 - NN Versi AVR ATmega8
              30 Oktober 2019 - NN Versi Arduino
              12 September 2018 - 8 Agustus 2020 - Observasi Plant
              17 Nopember 2020 - 19 Nopember 2020 - NN + Observasi Plant
              30 Januari 2024 - 31 Januari 2024 - Micro Python ESP32
Untuk       : Praktikum Kontrol Cerdas 2 POLITEKNIK ELEKTRONIKA NEGERI SURABAYA (PENS)

Perangkat   : Arduino Mega, Plant Emulasi, Koneksi ADC Emulasi dan Pin Port I/O (sebagai PWM) Emulasi
"""

import math
import random
import machine
import time

from ili9341 import Display, color565
from machine import Pin, SPI

spi = SPI(2, baudrate=32000000, sck=Pin(18), mosi=Pin(23), miso=Pin(19))
display = Display(spi, dc=Pin(33), cs=Pin(5), rst=Pin(32))
line=display.draw_line
cls=display.clear
RGB=color565

MinX=0
MinY=0
MaxX=239
MaxY=319

import tm1637

ss = tm1637.TM1637(clk=Pin(4), dio=Pin(2))


#line(0,0,200,100, color565(0,0,255))
#line(0,0,100,100, color565(0,255,0))
#line(0,0,100,200, color565(0,255,255))
#line(0,0,200,200, color565(255,255,0))
#line(MinX,MinY,MaxX,MaxY, RGB(255,255,255))

"""
v=0
x=0
y=0
i=0
while True:
  v+=(12-v)*0.01/(10*0.05)         # V = IT/C
  #print(v)
  line(239-x,int(y),238-x,int(v*10), RGB(255,0,0))
  x+=1
  if x>=239:
    x=0
    cls()
  y=v*10
  #sleep(1)
"""

uart=machine.UART(1, baudrate=115200, tx=16, rx=17)

MOTOR=3                                # Nomor PIN untuk output PWM (emulasi PWM oleh Arduino)
SENSOR=34#A0                              # Nomot PIN ADC

Kecepatan=1000
SettingPoint=1000
PWM=0
adc=1
Load=0                                     # Pembebanan, 0% = tanpa beban, 100% = beban maksimal
Periode=0                                  # panjang periode (Setting Point on/off) dalam satuan sampling, Periode=0 -> tidak periodik
Random=0                                   # nilai Setting Point acak
Index=0                                    # penghitung sampling (Loop)
SetP=1000                                     # BackUp Setting Point
RpmMax=2000                                # Display Max
Error=0
MSE=2e5

VinMin=0.25                             # Tegangan plant minimal, antara 0 volt sampai 5 volt
alfa=0.1                                # semakin kecil Alfa, TM semakin besar
KM=2000/5                               # konversi dari tegangan ke RPM
v1=0
v2=0
def plant(vin):                         # Plant Emulasi
  global v1, v2
  vin-=VinMin
  if vin<0: vin=0                       # dilengkapi dengan titik nyala minimal
  v1=vin*.7+v1*.3                       # orde #1
  v2=v1*alfa+v2*(1-alfa)                # orde #2
  return v2*KM                          # konversi dari tegangan ke RPM

out_plant=0
def _analogRead(ch):                    # ADC Emulasi
  return out_plant/KM*1023/5*(1.0-Load) # VRef 5 Volt resolusi 10 bit

def _analogWrite(pin, PWM):             # DAC Emulasi
  global out_plant
  out_plant = plant(PWM*5.0/255)        # VRef 5 Volt resolusi 8 bit

"""
Demo NN, SImple NN dengan
- 1 Input Node
- 1 LApis Hidden Layer, 5-beberapa Node
- 1 Output Neuron
- Domain Data [0,1]
"""

MaxHidden=50                              # Max Jumlah Node/Neuron pada Hidden Layer
LearningRate = 1                           # dapat diubah dengan perintah "R0.5"
nHidden = 5                                  # dapat diubah dengan perintah "H10"

InNN=0                                       # Data Masukan NN, 1 input
OutNN=0                                      # Data Keluaran NN, Output Neouron, setelah fungsi Aktifasi, 1 output
NH=[0]*MaxHidden                              # Keluaran Hidden Neuron, setelah Aktifasi
eH=[0]*MaxHidden                              # Error Propagasi Hidden Neuron, setelah Aktifasi
BH=[0]*MaxHidden                              # Bias dari Hidden Neuron
WH=[0]*MaxHidden                              # Bobot Masukan Hidden Neuron
WO=[0]*MaxHidden                              # Bobot Masukan dari Output Neuron
BO=0                                         # Bias Output Neuron
ErrNN=0                                      # Error untuk sistem update NN

def Init_NN():
  global WH, BH, WO, BO
  for h in range(MaxHidden):
    WH[h]=random.uniform(-1,1)                # Bangkitkan random untuk tiap bobot, [-1,1]
    BH[h]=0                                      # Nilai Awal Bias=0
    WO[h]=random.uniform(-1,1)                # Bangkitkan random untuk tiap bobot, [-1,1]
  BO=0

def sigmoid(x):
  if x>5:
    return 1                               # Untuk menghindarkan OverFlow,
  elif x<-5:
    return 0                         # Jika x>5 set y=1, Jika x<5, y=0
  else:
    return 1/(1+math.exp(-x))               # Diantara itu, hitung normal

def HitungMaju():
  global nHidden, InNN, WH, BH, NH, WO, BO, OutNN
  zo=0
  for h in range(nHidden):
    zh=InNN*WH[h]+BH[h]
    NH[h]=sigmoid(zh)                            # Hitung keluaran masing-masing Hidden Neuron  
    zo+=NH[h]*WO[h]                              # Sekaligus hitung masukan Sigma dari Output Neuron
  OutNN=sigmoid(zo+BO)                           # Hitung keluaran dari Output Neuron

def UpdateNN():
  #unsigned char h
  #float eo, eh
  global eH, WO, WH, BH, BO, ErrNN, OutNN, nHidden
  global LearningRate, NH, InNN
  eo=ErrNN*OutNN*(1-OutNN)                       # temporary, biar cepat
  for h in range(nHidden):
    eH[h]=WO[h]*eo                               # selesaikan sebelum update WO
    WO[h]+=LearningRate*eo*NH[h]                 # update bobot output
    eh=eH[h]*NH[h]*(1-NH[h])                     # temporary, biar cepat
    WH[h]+=LearningRate*eh*InNN
    BH[h]+=LearningRate*eh
  BO+=LearningRate*eo

#randomSeed(analogRead(0))                      # atur nilai awal bilangan random, dari ADC
Init_NN()


x=0
SP=0        # Setting Point lalu
K=0         # Kecepatan lalu
P=0         # PWM lalu
E=0         # Error lalu
M=0         # MSE lalu

def run():
  while True:                                       # Bagian yang dikerjakan secara terus-menerus oleh Arduino
    if uart.any():                          # Apakah ada penerimaan data serial dari PC ?
      uart.read(d)
      if d=='s':                                   # Jika Ya, apakah data dimulai dengan huruf depan 's' ?
        SetP=Serial.parseInt()                   # Ubah Setting Point, "s1000"
        if(SetP<0): SetP=0
        elif(SetP>RpmMax): SetP=RpmMax
        Index=0
      elif d=='a':                                   # Ubah Timing plant, "a0.0" - "a1.0" alfa 0.00 sampai alfa 1.00
        alfa=Serial.parseFloat()
        if(alfa<0): alfa=0
        elif(alfa>1): alfa=1
      elif d=='l':                                   # Ubah beban Plant, "l0" - "l100" load mati (0%) sampai load 100%
        Load=Serial.parseInt()/100.0
        if(Load<0): Load=0
        elif(Load>1): Load=1
      elif d=='p':                                   # 'p0' periodik mati, 'p100' periodik setiap 100 siklus (sampling)
        Periode=Serial.parseInt()
        Periode=(Periode/2)*2                    # harus genap
        if(Periode<0): Periode=0
        elif(Periode>1000): Periode=1000
        Index=0
      elif d=='r':                                   # 'r0' random mati, 'r1' random aktif
        Random=Serial.parseInt()
        if(Random<0): Random=0
        elif(Random>1): Random=1
        randomSeed(analogRead(0))
      elif d=='v':                                   # 'v0' ... 'v5' nilai Vmin dari 0 volt sampai 5 volt
        VinMin=Serial.parseFloat()
        if(VinMin<0): VinMin=0
        elif(VinMin>5): VinMin=5
      elif d=='m':                                   # 'r0' random mati, 'r1' random aktif
        RpmMax=Serial.parseInt()
        if(RpmMax<0): RpmMax=0
        elif(RpmMax>10000): RpmMax=10000
      elif d=='R':                                   # Ubah Learning Rate, "R0.0" - "R10.0", jangan terlalu besar
        LearningRate=Serial.parseFloat()
        if(LearningRate<0): LearningRate=0
      elif d=='H':                                   # Ubah Jumlah Neuron pada Hidden Layer, "H1" - "H50"
        nHidden=Serial.parseInt()
        if(nHidden<0): nHidden=0
        elif(nHidden>MaxHidden): nHidden=MaxHidden
      elif d=='I':                                   # Inisialisasi ulang NN, "I"
        Init_NN()
    
                                                 # Misal data yang diterima, "s1000"<CR>
                                                  # Set Carriage Return pada terminal, agar saat enter, lansung dikirimkan CR  
    if(Index==0):
      if Random==0: SettingPoint=SetP
      else: SettinPoint=random(0,RpmMax)
    if(Periode>0 and Index*2==Periode):
      if Random==0: SettingPoint=0
      else: SettingPoint=random(0,RpmMax)

    adc=_analogRead(SENSOR)                        # Baca tegangan sensor dari PORT ADC, tegangan sensor sebanding dengan kecepatan/keluaran PLANT
    Kecepatan=adc*RpmMax/1023                      # Konversi dari ADC ke Kecepatan (dari data biner ADC ke RPM)
  
    Error=SettingPoint-Kecepatan
    MSE=Error*Error*0.02+MSE*0.98
  
    #InNN=SettingPoint/RpmMax                     # Open Loop, InNN=[0,1], diambil dari SettingPoint=[0,2000]
    InNN=Error/(2*RpmMax)+0.5                      # CloseLoop, InNN=[0,1], diambil dari Error=[-2000,2000]
    #InNN=Kecepatan/RpmMax                        # Recurrent, InNN dari output Kecepatan
  
    HitungMaju()                                   # Hitung dari InNN[0,1] ke OutNN[0,1]
  
    ErrNN=Error/RpmMax                             # Error untuk Update NN, nilai [-1,1], dari Error[-2000,2000]
    UpdateNN()                                     # Dari ErrNN, lakukan Update secara OnLine

    PWM=OutNN*255                                  # Tentukan nilai PWM (data biner) berdasarkan Setting Point yang telah ditentukan
    if(PWM<0): PWM=0
    if(PWM>255): PWM=255
    _analogWrite(MOTOR,PWM)                        # Kirim ke PLANT melalui PORT I/O (sinyal PWM yang di-emulasi dalam bentuk program dalam Arduino)
 
    print("SetPoint=", SettingPoint,sep='', end=' ')                      # Tampilkan SetPoint ke Keterangan, SetPoint=1000
    print(" Kecepatan=", Kecepatan,sep='', end=' ')                    # Tampilkan Kecepatan
    print(" PWM=", PWM,sep='', end=' ')
    print(" Error=", Error,sep='', end=' ')
    print(" MSE=", MSE,sep='', end=' ')
    print(" Max=", RpmMax,sep='', end=' ')
    print(" Hidden=", nHidden,sep='', end=' ')
    print(" LR=", LearningRate, sep='')
 

    #print(SettingPoint,sep='', end=' ')                   # Kirim data ke program monitor melalui komunikasi serial
    #print(Kecepatan,sep='', end=' ')                      # Format Protokol: "1000 850 87 2000"<CR>
    #print(PWM,sep='', end=' ')                            # tanda 0 artinya tidak ada nilai pecahan
    #if Error>RpmMax: print(RpmMax,sep='', end=' ')
    #elif Error<0: print(0,sep='', end=' ')
    #else: print(Error,sep='', end=' ')
    #if MSE/100>RpmMax: print(RpmMax,sep='', end=' ')
    #else: print(MSE/1000,sep='', end=' ')
    #print(RpmMax)                         # akhir data serial, dengan karakter <LF-CR>

    ss.number(int(Kecepatan))

    if x<239:
      line(239-x,int(SP),238-x,int(SettingPoint/10), RGB(0,0,255))
      SP=SettingPoint/10
      line(239-x,int(K),238-x,int(Kecepatan/10), RGB(255,0,0))
      K=Kecepatan/10
      line(239-x,int(P),238-x,int(PWM), RGB(0,255,0))
      P=PWM
      line(239-x,int(E),238-x,int(Error/10), RGB(0,255,255))
      E=Error/10
      line(239-x,int(M),238-x,int(MSE/1000), RGB(255,255,0))
      M=MSE/1000
      x+=1

    if(Periode>0):                                   # Jika setting point dibuat periodik
      Index+=1
      if(Index>=Periode): Index=0
  
    time.sleep_ms(10)                                      # Atur waktu sampling

run()