from machine import Pin
import qwiic_ism330dhcx
import sys
import time
from time import sleep_ms, ticks_ms, ticks_diff
import math
import network, espnow, struct
class QuadratureEncoder: #klasa utworzona w celu zliczania impulsów enkoderów - ściągnięta z internetu
    def __init__(self, pin_a_num, pin_b_num, pullups=True):
        pull = Pin.PULL_UP if pullups else None
        self.a = Pin(pin_a_num, Pin.IN, pull)
        self.b = Pin(pin_b_num, Pin.IN, pull)
        self._pos = 0

        
        trig = Pin.IRQ_RISING | Pin.IRQ_FALLING
        try:
            
            self.a.irq(trigger=trig, handler=self._cb_a, hard=True)
            self.b.irq(trigger=trig, handler=self._cb_b, hard=True)
        except TypeError:
            # fallback: soft IRQ
            self.a.irq(trigger=trig, handler=self._cb_a)
            self.b.irq(trigger=trig, handler=self._cb_b)

    def _cb_a(self, pin):
        #kierunek zależy od relacji A do B
        forward = pin.value() ^ self.b.value()
        self._pos += 1 if forward else -1

    def _cb_b(self, pin):
        forward = self.a.value() ^ pin.value() ^ 1
        self._pos += 1 if forward else -1

    def read(self):
        return self._pos

    def reset(self, value=0):
        self._pos = value


def kalibracja(myIsm, n=300, dt=0.1):
    sx = sy = sz = 0.0
    sgx = sgy = sgz = 0.0
    cnt = 0

    while cnt<n:
        if myIsm.check_status():
            a = myIsm.get_accel()
            g = myIsm.get_gyro()
            sx  += a.xData; sy  += a.yData;
            sgz += g.zData
            cnt+=1;
        time.sleep(dt)
    
    ax0 = sx / n
    ay0 = sy / n
    gz0 = sgz / n
    return (ax0, ay0, gz0)

sta = network.WLAN(network.STA_IF)
sta.active(False)
sta.active(True)
sta.disconnect()

e = espnow.ESPNow()
e.deinit()
e.active(True)


PEER_A = b'D\x17\x93%\x08g'
e.add_peer(PEER_A)

FMT_C2A = "<2sBHfff"     
MAGIC = b"SC"           
VER = 1
seq = 0
################################################################################
myIsm = qwiic_ism330dhcx.QwiicISM330DHCX()
if myIsm.is_connected() == False:
    print("The device isn't connected to the system. Please check your connection", \
        file=sys.stderr)
myIsm.begin()
myIsm.device_reset()
#czekamy aż się zresetuje
while myIsm.get_device_reset() == False:
    time.sleep(1)

print("Reset.")
print("Applying settings.")
time.sleep(0.100)

myIsm.set_device_config()
myIsm.set_block_data_update()

#ustawienie precyzji i zakresu
myIsm.set_accel_data_rate(myIsm.kXlOdr104Hz)
myIsm.set_accel_full_scale(myIsm.kXlFs4g)

myIsm.set_gyro_data_rate(myIsm.kGyroOdr104Hz)
myIsm.set_gyro_full_scale(myIsm.kGyroFs500dps)

#wewnętrzny filtr
myIsm.set_accel_filter_lp2()
myIsm.set_accel_slope_filter(myIsm.kLpOdrDiv100)

myIsm.set_gyro_filter_lp1()
myIsm.set_gyro_lp1_bandwidth(myIsm.kBwMedium)

ax0, ay0, wz0 = kalibracja(myIsm) #tu się czeka pół minuty   
ax =0; ay=0; wz=0;
#wartości początkowe:
w1=0;
alfa0=0;


vx0=0;
vx1=0;
vx2=0;
ax1=0;
x0=0;

vy0=0;
vy1=0;
vy2=0;
ay1=0;
y0=0;
dt_imu=0.1;
#do enkoderów:
PPR_WHEEL = 823.1                             
COUNTS_PER_REV = 4*PPR_WHEEL  
WHEEL_DIAM_M = 0.070
WHEEL_CIRC_M = math.pi * WHEEL_DIAM_M
S1ENC_A_PIN = 19 
S1ENC_B_PIN = 16
S2ENC_A_PIN = 18 
S2ENC_B_PIN = 17 

enc1 = QuadratureEncoder(S1ENC_A_PIN, S1ENC_B_PIN, pullups=True)
enc2 = QuadratureEncoder(S2ENC_A_PIN, S2ENC_B_PIN, pullups=True)
t_prev = ticks_ms()
period_ms = 100   
while True:
    sleep_ms(5)
    t = ticks_ms()
    dt_ms = ticks_diff(t, t_prev)
    if dt_ms >= period_ms:
        t_prev = t
        dt = dt_ms / 1000.0
        c1 = enc1.read()       
        c2 = enc2.read()
        if c1 != last1 and c2!=last2;
            dc1 = c1 - prev1
            dc2 = c2 - prev2
            prev1, prev2 = c1, c2
            last1, last2 = c1, c2
            rps1 = (dc1 / COUNTS_PER_REV) / dt
            rps2 = (dc2 / COUNTS_PER_REV) / dt
            rpm1 = rps1 * 60.0
            rpm2 = rps2 * 60.0
            vms1 = rps1 * WHEEL_CIRC_M
            vms2 = rps2 * WHEEL_CIRC_M
            vcom=(vms1+vms2)/2
            print("vcom", vcom)
            payload = struct.pack(FMT_C2A, MAGIC, VER, seq, rpm1, rpm2, vcom)
            e.send(PEER_A, payload, True)
            seq = (seq + 1) & 0xFFFF
        elif c1 == last1 or c2 == last2:
            t=False
   # if __name__ == '__main__':
        #try:
    if t==True: #w celu ograniczenia dryfu wyznaczamy pozycję z IMU tylko, gdy robot się faktycznie porusza
        if myIsm.check_status():
            #zbieramy potrzebne nam dane z IMU:
            accelData = myIsm.get_accel()
            ax=accelData.xData-ax0;
            ay=accelData.yData-ay0;
            gyroData = myIsm.get_gyro()
            wz=gyroData.zData-wz0;
            #print(ax, ay, wz)
           # except (KeyboardInterrupt, SystemExit) as exErr:
             #   print("\nEnding Example")
             #   sys.exit(0)
        ax=ax/1000; ay=ay/1000; wz=(wz/1000)*(math.pi/180);
        ax2=ax;
        ay2=ay;
        w2=wz;
        alfa=(w2+w1)*0.5*dt_imu+alfa0;
        alfa0=alfa; #kąt obrotu układu współrzędnych względem pierwszego
        w1=w2;
    
        vx1=vx2;
        vx=(ax2+ax1)*0.5*dt_imu+vx0;
        vx0=vx;
        vx2=vx;
        ax1=ax2;
        xm=x0;
        x=(vx1+vx2)*0.5*dt_imu#+x0;
        x0=x;
        xn=x;
    
        vy1=vy2;
        vy=(ay2+ay1)*0.5*dt_imu+vy0;
        vy0=vy;
        vy2=vy;
        ay1=ay2;
        ym=y0; #=yn
        y=(vy1+vy2)*0.5*dt_imu#+y0;
        y0=y;
        yn=y;
        xk=xm+math.cos(alfa)*xn-math.sin(alfa)*yn; # ustalona pozycja x w układzie globalnym związanym z miejscem startu robota
        yk=ym+math.sin(alfa)*yn+math.cos(alfa)*xn; # ustalona pozycja y
        print(xk, yk);
    payload = struct.pack(FMT_C2A, MAGIC, VER, seq, xk, yk, vcom)
    e.send(PEER_A, payload, True)
    seq = (seq + 1) & 0xFFFF