#include <Arduino.h>
#include <stdint.h>
#include "amswirelib.hpp"
#include "amsgenlib.hpp"

#include "amsswcomm.hpp"

//initialize the sw communication channel
// each swcomm channel has 3 pins, the clock pin (controlled by the master),
// the input pin (receiving a signal from the other device)
// and an output pin (sending a signal to the other device)
//Inputs:
//  ms - master or slave
//  clkpin - the clock pin number
//  outpin - the output pin number
//  inpin - the input pin number
//  divisor - how many updates to skip between clock changes
void amsswcomm::init(uint8_t _ms, uint8_t _clkpin, uint8_t _sendpin, uint8_t _recvpin, int _divisor)
{

    ms = _ms;
    clkpin = _clkpin;
    sendpin = _sendpin;
    recvpin = _recvpin;


    divisor = _divisor;
    clockticks = 0;
    clockstate = 0;
    lastclockstate = 0;

    if(ms==1)
    {
        //master
        set_pinmode(clkpin,1);
        set_pinmode(recvpin,0);
        set_pinmode(sendpin,1);

    }
    else if(ms==0)
    {
        //slave
        set_pinmode(clkpin,0);
        set_pinmode(recvpin,0);
        set_pinmode(sendpin,1);

    }

    clearrecvqueue();
    clearsendqueue();

    // recvmessage_size = -1;
    // sendmessage_size = -1; //no message
    // recvmessage_bit = 0;
    // sendmessage_bit = 0;

    // sendstate = 0;
    // recvstate = 0;
    // sendbytepointer = 0;
    // recvbytepointer = 0;
    // sendbitpointer = 0;
    // recvbitpointer = 0;

    int8_t I;
    for(I=0;I<AMSSWCOMM_BUFFSIZE;I++)
    {
        sendmessage[I] = 0;
        recvmessage[I] = 0;
    }



    return;
}

void amsswcomm::clearrecvqueue()
{
    int8_t I;

    recvmessage_size = -1;
    recvmessage_byte = 0;
    recvstate = 0;
    recvbytepointer = 0;
    recvbitpointer = 0;

    // for speed, deal with this by sendmessage_size
    // for(I=0;I<AMSSWCOMM_BUFFSIZE;I++)
    // {
    //     recvmessage[I] = 0;
    // }

    

    return;
}

void amsswcomm::clearsendqueue()
{
    int8_t I;

    sendmessage_size = -1;
    sendmessage_byte = 0;
    sendstate = 0;
    sendbytepointer = 0;
    sendbitpointer = 0;

    // for speed, deal with this by recvmessage_size
    // for(I=0;I<AMSSWCOMM_BUFFSIZE;I++)
    // {
    //     sendmessage[I] = 0;
    // }

    return;
}

void amsswcomm::update_step()
{

    if(ms==1)
    {
        //if master

        if(clockticks<divisor)
        {
            clockticks++;
        }
        else
        {
            clockticks = 0;
            
            lastclockstate = clockstate;
            clockstate = !lastclockstate;
            write_pin(clkpin,clockstate);
            
            //update_send triggers on rising edge of clkpin
            if(clockstate==1 && lastclockstate==0)
            {
                update_send();
            }
            //update recv triggers on falling edge of clkpin
            if(clockstate==0 && lastclockstate==1)
            {
                update_recv();
            }


        }
    }
    else if(ms==0)
    {
        //if servant

            //check clock every passthrough

        lastclockstate = clockstate;
        clockstate = read_pin(clkpin);

        //update_send triggers on rising edge of clkpin
        if(clockstate==1 && lastclockstate==1)
        {
            update_send();
        }
        //update recv triggers on falling edge of clkpin
        if(clockstate==0 && lastclockstate==1)
        {
            update_recv();
        }
    }

    return;
}

//called by update_step
void amsswcomm::update_send()
{

    uint8_t bit;

    if(sendstate==0)
    {
        //do nothing, no message
        write_pin(sendpin,0); //send zeroes until messaging
    }
    else if(sendstate==1)
    {
        //send starting byte
        sendmessage_byte = 0b0000001111111111;

        bit = (sendmessage_byte & 1<<(9-sendbitpointer)) >> (9-sendbitpointer);
        write_pin(sendpin,bit);

        sendbitpointer++; //advance pointer
        if(sendbitpointer>=10)
        {
            //if finished, advance state
            sendbitpointer=0;
            sendbytepointer=0;
            sendstate=2;
        }
    }
    else if(sendstate==2)
    {
        if(sendmessage_size>=0 && sendbytepointer<sendmessage_size)
        {
            sendmessage_byte = 0b0000001100000000 + sendmessage[sendbytepointer];

            bit = (sendmessage_byte & 1<<(9-sendbitpointer)) >> (9-sendbitpointer);
            write_pin(sendpin,bit);

            sendbitpointer++; //advance pointer
            if(sendbitpointer>=10)
            {
                //if finished, advance bytepointer
                sendbitpointer=0;
                sendbytepointer++;
            }
            if(sendbytepointer>sendmessage_size)
            {
                //if finished with message, advance state to send ending byte
                sendbitpointer=0;
                sendbytepointer=0;
                sendstate=3;
            }
        }
        else
        {
            //size 0 message
            sendbitpointer=0;
            sendbytepointer=0;
            sendstate=3;
        }
    }
    else if(sendstate==3)
    {
        //send ending byte
        sendmessage_byte = 0; //if zero is received for more than 10 bits, end message

        bit = (sendmessage_byte & 1<<(9-sendbitpointer)) >> (9-sendbitpointer);
        write_pin(sendpin,bit);

        sendbitpointer++; //advance pointer
        if(sendbitpointer>=10)
        {
            //if finished, advance state
            sendbitpointer=0;
            sendbytepointer=0;
            sendstate=4;
        }
    }
    else if(sendstate==4)
    {
        clearsendqueue();
    }
    else
    {
        //this shouldn't happen, but if it does, reset things
        clearsendqueue();
    }

    return;
}

void amsswcomm::update_recv()
{
    uint8_t bit;

    if(recvstate==0)
    {
        bit = read_pin(recvpin);
        if(bit==1)
        {
            recvstate=1; //advance to receive state
            recvbitpointer=1;
            recvmessage_byte = 0b0000001000000000; //set first leading bit
            recvbytepointer = 0;
        }
    }
    else if(recvstate==1)
    {
        bit = read_pin(recvpin);
        recvmessage_byte = recvmessage_byte + (bit<<(9-recvbitpointer));
        recvbitpointer++;

        if(recvbitpointer>=10)
        {
            recvbitpointer = 0;
            if(recvmessage_byte = 0b0000001111111111)
            {
                recvstate = 2;
                recvmessage_byte = 0;
                recvbytepointer = 0;
                recvmessage_size = 0;
            }
            else
            {
                //byte received not part of message or garbled message, ignore
                recvstate = 0;
                recvbytepointer = 0;
                clearrecvqueue();
            }
        }
    }
    else if(recvstate==2)
    {
        bit = read_pin(recvpin);
        recvmessage_byte = recvmessage_byte + (bit<<(9-recvbitpointer));
        recvbitpointer++;

        if(recvbitpointer>=10)
        {
            if(recvmessage_byte==0)
            {
                //end of message byte has been received
                recvbitpointer = 0;
                recvbytepointer = 0;
                recvstate = 3; //has message
                recvmessage_byte = 0;
            }
            else
            {
                recvbitpointer = 0;
                recvmessage[recvbytepointer] = (uint8_t)(recvmessage_byte & 0b0000000011111111);
                recvbytepointer++;
                recvmessage_size++;
                recvmessage_byte = 0;
            }
        }
    }
    else if(recvstate==3)
    {
        //has message, do nothing and wait until message has been processed
    }
    else
    {
        clearrecvqueue();
    }

    return;
}

bool amsswcomm::has_message()
{
    if(recvstate==3)
    {
        return 1;
    }
    return 0;
}

//receives the message and resets the receive queue
//return value is message size, or -1 for no message
int amsswcomm::recv_message(char *outputbuff, int outputbuffsize)
{
    int ret = -1;
    int I;

    if(has_message())
    {
        ret = recvmessage_size;
        for(I=0;I<recvmessage_size && I<outputbuffsize && I<AMSSWCOMM_BUFFSIZE; I++)
        {
            outputbuff[I] = recvmessage[I];
        }
        if(I<outputbuffsize) outputbuff[I] = 0; //NULL terminate if possible to help string logic

        for(I=0;I<AMSSWCOMM_BUFFSIZE;I++)
        {
            recvmessage[I] = 0;
        }
        recvmessage_size = -1;
        // recvmessage_byte = 0;
        // recvstate = 0;
        // recvbytepointer = 0;
        // recvbitpointer = 0;
        clearrecvqueue();
    }

    return ret;
}

//places a message in the send queue
//1 - success
//0 - failure - a message is being sent, must wait to send
bool amsswcomm::send_message(char *inputbuff, int messagesize)
{
    bool ret = 0;
    int I;

    if(sendstate==0)
    {
        ret = 1;
        if(messagesize<=AMSSWCOMM_BUFFSIZE) sendmessage_size = messagesize; else sendmessage_size = AMSSWCOMM_BUFFSIZE;
        for(I=0;I<sendmessage_size;I++)
        {
            sendmessage[I] = inputbuff[I];
        }
        for(I=sendmessage_size;I<AMSSWCOMM_BUFFSIZE;I++)
        {
            sendmessage[I] = 0;
        }

        sendstate = 1; //start sending message
        sendmessage_byte = 0;
        sendbitpointer = 0;
        sendbytepointer = 0;

    }

    return ret;
}