#!/usr/bin/python

import os,sys,math
import numpy as np

#whatever arduino GPIO library 
import RPi.GPIO as GPIO
import time




## dummy functions for now - work through the logic while you still have it in your head!
def set_pinmode(pinnumber, mode):

    #set GPIO mode to BOARD
    if(mode==1):
        GPIO.setup(pinnumber, GPIO.OUT)    
    elif(mode==0):
        GPIO.setup(pinnumber, GPIO.IN)
        
    return

def read_pin(pinnumber):
    ret = 0
    val = GPIO.input(pinnumber)
    ret = int(val)

    return ret

def write_pin(pinnumber, val):
    #print("dummy: sending {} on {}".format(pinnumber, val))

    if(val==1):
        GPIO.output(pinnumber,GPIO.HIGH)
    else:
        GPIO.output(pinnumber,GPIO.LOW)

    return


#3-wire software-defined terminal class
#mirrors the logic of the arduino class, for talking with 
#arduino-nano controlled perhipheral devices
class amsswcomm():

    def __init__(self):



        return
    
    def comminit(self, _ms, _clkpin, _sendpin, _recvpin, _divisor = 1):

        self.ms = int(_ms)
        self.clkpin = int(_clkpin)
        self.sendpin = int(_sendpin)
        self.recvpin = int(_recvpin)
        self.divisor = _divisor


        self.sendmessage = np.uint8([])
        self.recvmessage = np.uint8([])
        
        self.clearrecvqueue()
        self.clearsendqueue()

        self.clockstate = 0
        self.lastclockstate = 0
        self.clockticks = 0

        if(self.ms==1):
            #master, most often the case for the raspberry pi
            set_pinmode(self.clkpin,1)
            set_pinmode(self.recvpin,0)
            set_pinmode(self.sendpin,1)

        elif(self.ms==0):
            #servant
            set_pinmode(self.clkpin,0)
            set_pinmode(self.recvpin,0)
            set_pinmode(self.sendpin,1)

        else:
            pass



        return

    def update_step(self):

        if(self.ms==1):
            #master, most often the case for the raspberry pi
            if(self.clockticks<self.divisor):
                self.clockticks = self.clockticks+1
            else:
                self.clockticks = 0

                self.lastclockstate = self.clockstate
                self.clockstate = int(not self.lastclockstate)
                
                write_pin(self.clkpin,self.clockstate)

                if(self.clockstate==1 and self.lastclockstate==0):
                    self.update_send()
                if(self.clockstate==0 and self.lastclockstate==1):
                    self.update_recv()
    
        elif(self.ms==0):

            self.lastclockstate = self.clockstate
            self.clockstate = read_pin(self.clkpin)

            if(self.clockstate==1 and self.lastclockstate==0):
                self.update_send()
            if(self.clockstate==0 and self.lastclockstate==1):
                self.update_recv()

        else:
            pass

        return
    
    def clearrecvqueue(self):

        self.recvmessage_byte = np.uint16(0)
        self.recvbytepointer = 0
        self.recvbitpointer = 0
        self.recvstate = 0
        self.recvmessage = np.uint8([])

        return
    
    def clearsendqueue(self):

        self.sendmessage_byte = np.uint16(0)
        self.sendbytepointer = 0
        self.sendbitpointer = 0
        self.sendstate = 0
        self.sendmessage = np.uint8([])

        return

    def update_send(self):

        if(self.sendstate==0):
            #do nothing, no message in queue
            write_pin(self.sendpin,0)
        elif(self.sendstate==1):
            self.sendmessage_byte = 0b0000001111111111
            bit = (self.sendmessage_byte & 1<<(9-self.sendbitpointer)) >> (9-self.sendbitpointer)
            
            write_pin(self.sendpin,bit)

            self.sendbitpointer = self.sendbitpointer+1
            if(self.sendbitpointer>=10):
                self.sendbitpointer = 0
                self.sendbytepointer = 0
                self.sendstate = 2

        elif(self.sendstate==2):
            #send message byte
            if(len(self.sendmessage)>=0):
                self.sendmessage_byte = 0b0000001100000000 + self.sendmessage[self.sendbytepointer]
                bit = (self.sendmessage_byte & 1<<(9-self.sendbitpointer)) >> (9-self.sendbitpointer)
                write_pin(self.sendpin,bit)

                self.sendbitpointer = self.sendbitpointer + 1
                if(self.sendbitpointer>=10):
                    self.sendbitpointer = 0
                    self.sendbytepointer = self.sendbytepointer + 1
                if(self.sendbytepointer>=len(self.sendmessage)):
                    self.sendbitpointer = 0
                    self.sendbytepointer = 0
                    self.sendstate = 3
            else:
                #size 0 message
                self.sendbitpointer = 0
                self.sendbytepointer = 0
                self.sendstate = 3
            
        elif(self.sendstate==3):
            #send ending byte
            self.sendmessage_byte = 0
            bit = (self.sendmessage_byte & 1<<(9-self.sendbitpointer)) >> (9-self.sendbitpointer)
            write_pin(self.sendpin,bit)

            self.sendbitpointer = self.sendbitpointer+1
            if(self.sendbitpointer>=10):
                self.sendbitpointer = 0
                self.sendbytepointer = 0
                self.sendstate = 4


        elif(self.sendstate==4):
            #Finish and reset
            self.clearsendqueue()
        else:
            #improper condition, should not occur
            self.clearsendqueue()

        return
    
    def update_recv(self):

        if(self.recvstate==0):
            bit = read_pin(self.recvpin)
            if(bit==1):
                self.recvstate = 1
                self.recvbitpointer = 1
                self.recvmessage_byte = 0b0000001000000000
                self.recvbytepointer = 0
                
        elif(self.recvstate==1):
            bit = read_pin(self.recvpin)
            self.recvmessage_byte = self.recvmessage_byte + (bit<<(9-self.recvbitpointer));
            self.recvbitpointer = self.recvbitpointer + 1

            if(self.recvbitpointer>=10):
                self.recvbitpointer = 0
                if(self.recvmessage_byte == 0b0000001111111111):
                    self.recvstate = 2
                    self.recvbytepointer = 0

                    #append to message
                    #byte2 = np.uint8(self.recvmessage_byte & 0b0000000011111111)
                    #self.recvmessage = np.append(self.recvmessage,byte2) 
                    
                    self.recvmessage_byte = 0
                else:
                    #garbled message
                    self.clearrecvqueue()
        
        elif(self.recvstate==2):
            #receive messsage byte
            bit = read_pin(self.recvpin)
            self.recvmessage_byte = self.recvmessage_byte + (bit<<(9-self.recvbitpointer));
            self.recvbitpointer = self.recvbitpointer + 1

            if(self.recvbitpointer>=10):
                self.recvbitpointer = 0
                
                if(self.recvmessage_byte==0):
                    #end of message byte
                    self.recmessage_byte = 0
                    self.recvstate = 3
                else:
                    #append to message
                    byte2 = np.uint8(self.recvmessage_byte & 0b0000000011111111)
                    self.recvmessage = np.append(self.recvmessage,byte2) 
                    self.recvmessage_byte = 0

                    #DEBUG
                    # if(len(self.recvmessage)>10):
                    #     self.recmessage_byte = 0
                    #     self.recvstate = 3

        elif(self.recvstate==3):
            #has a message, - wait until this is handled
            pass
        else:
            #invalid state
            self.clearrecvqueue()

        return

    def has_message(self):
        ret = False

        if(self.recvstate==3):
            ret = True

        return ret

    def recv_message(self):

        message = None
        if(self.has_message()):
            message = self.recvmessage
            self.clearrecvqueue()

        return message
    
    def send_message(self,message):

        ret = False

        if(type(message) is type(str)):
            message = message.decode("utf-8")
        if(type(message) is type(bytes)):
            message = np.frombuffer(message,dtype=np.uint8)

        if(self.sendstate==0):
            ret = True
            self.sendmessage = message
            self.sendstate = 1
            self.sendmessage_byte = 0
            self.sendbitpointer = 0
            self.sendbytepointer = 0
        
        return ret