from machine import Pin
from machine import PWM
from time import sleep
import gc
from rp2 import *

# Enable debug messages over serial (a bit slower)
DEBUG = 0
DEBUG2 = 0
# Enable canbus output
CANBUS = 1

CAN1 = CAN()
FAN1 = PWM(Pin(20))
FAN1.freq(25000)

LED1 = Pin(25, Pin.OUT)

IN1 = Pin(16,Pin.OUT)
IN2 = Pin(17,Pin.OUT)
IN3 = Pin(18,Pin.OUT)
IN4 = Pin(19,Pin.OUT)

PINS = [IN1, IN2, IN3, IN4]

sequence = [[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]]
sequence_reverse = [[0,0,0,1],[0,0,1,0],[0,1,0,0],[1,0,0,0]]
allpinsoff = [0,0,0,0]

FAN1.duty_u16(25000)
sleep(5)
FAN1.duty_u16(0)
sleep(2)

# CIRCLE = 1024
CURRENTSTEP=0
NEXTSTEP=0
FORWARD=0
REVERSE=0
TMIN=18
TMAX=33
# 18=0
# x=((1024/25)*(t-TMIN)):1
#x=((1024/25)*(t-TMIN)):1

while True:
    if CANBUS:
        frames=CAN1.recv()
        for frame in frames:
            mycid=frame.get_arbitration_id()
            if mycid == 48:            ######################################
                rawdata=frame.get_data()
                lefttemp=int(((rawdata[0])/2.4))
                righttemp=int(((rawdata[1])/2.4))
                combtemp=int(((rawdata[2])/2.4))
                #print(combtemp)
                #
                fanspeed=rawdata[4]

                # fan handling
                #print(fanspeed)
                # fanspeed is 0-255
                # transform into duty cycle
                # 0 is 5000
                # 255 is 25000
                # aka 20000 range
                if fanspeed == 0:
                    dutycycle=0
                    FAN1.duty_u16(dutycycle)
                else:
                    # approximation ... 19890 = 24890 ~ 25000	
                    dutycycle=( 5000 + ( 78 * int(fanspeed) ) )
                    FAN1.duty_u16(dutycycle)
                print(dutycycle)
                
                # temp handlingv
                NEXTSTEP=int(combtemp*1024/100)
                if NEXTSTEP > CURRENTSTEP:
                    FORWARD=1
                    count=int(NEXTSTEP-CURRENTSTEP)
                    while count > 0:
                        for step in sequence:
                            for i in range(len(PINS)):
                                PINS[i].value(step[i])
                                sleep(0.001)
                            count=count-1
                else:
                    FORWARD=0
                    count=int(CURRENTSTEP-NEXTSTEP)
                    while count > 0:
                        for step in sequence_reverse:
                            for i in range(len(PINS)):
                                PINS[i].value(step[i])
                                sleep(0.001)
                            count=count-1
                #
                for i in range(len(PINS)):
                    PINS[i].value(allpinsoff[i])
                CURRENTSTEP=NEXTSTEP
                
        LED1.toggle()
                    
    else:
        LED1.toggle()
        #print(frame)
        #x=frame.get_data()
        #int result = ((0xF0 - 0x10) / 15) * (value - 15) + 0x10;
        # x / (0xF0/0x10) = (y - 15) + 0x10
        #lefttemp=int((x[0]/(224/16))+15-1)
        #righttemp=int((x[1]/(224/16))+15-1)
        #fanspeed=x[4]
        count=512
        while count > 0:
            for step in sequence:
                for i in range(len(PINS)):
                    PINS[i].value(step[i])
                    sleep(0.001)
                count=count-1
        #         print(lefttemp)
        #        print(righttemp)
        #        print(fanspeed)
        for i in range(len(PINS)):
            PINS[i].value(allpinsoff[i])
        sleep(0.2)
        count=512
        while count > 0:
            for step in sequence_reverse:
                for i in range(len(PINS)):
                    PINS[i].value(step[i])
                    sleep(0.001)
                count=count-1
        for i in range(len(PINS)):
            PINS[i].value(allpinsoff[i])        
    sleep(1)
    if DEBUG:
        # debug output on serial is slower
        sleep(0.05)