/**
 * This is a very small example that shows how to use
 * protothreads. The program consists of two protothreads that wait
 * for each other to toggle a semaphore.
 *
 * Modified by Bruce Land to make a more compliant example
 * Aug 2014
 */

/* We must always include pt.h in our protothreads code. */
#include <plib.h>
#include "pt.h"
#include "pt-sem.h"
#include <stdio.h> 

#pragma config FNOSC = FRCPLL, POSCMOD = HS, FPLLIDIV = DIV_2, FPLLMUL = MUL_15, FPBDIV = DIV_2, FPLLODIV = DIV_1
#pragma config FWDTEN = OFF
// frequency we're running at
#define	SYS_FREQ 60000000

// UART parameters
#define BAUDRATE 9600 // must match PC end
#define PB_DIVISOR (1 << OSCCONbits.PBDIV) // read the peripheral bus divider, FPBDIV
#define PB_FREQ SYS_FREQ/PB_DIVISOR // periperhal bus frequency
#define clrscr() printf( "\x1b[2J")
#define home()   printf( "\x1b[H")
#define pcr()    printf( '\r')
#define crlf     putchar(0x0a); putchar(0x0d);
#define backspace 0x7f // make sure your backspace matches this!
#define max_chars 32 // for input buffer

// === thread structures ============================================
// semaphores for controlling two threads
static struct pt_sem control_t1, control_t2 ;
// thread control structs
static struct pt pt1, pt2, pt3, pt_input ;

// macro to time a thread execution interveal
#define PT_YIELD_TIME(delay_time) \
    do { static int time_thread; \
    PT_YIELD_UNTIL(pt, milliSec >= time_thread); \
    time_thread = milliSec + delay_time ;} while(0);

//====================================================================
// === Timer 2 interrupt handler =====================================
// ipl2 means "interrupt priority level 2"
// ASM output is 47 instructions for the ISR
volatile int milliSec ;
void __ISR(_TIMER_2_VECTOR, ipl2) Timer2Handler(void)
{
    // clear the interrupt flag
    mT2ClearIntFlag();
    // keep time
    milliSec++ ;
}

// estimate microSec since start up
long long uSec(void)
{
    return (long long)milliSec * 1000 + (long long)ReadTimer2()/30 ;
}

//====================================================================
// build a string from the UART2 /////////////
//////////////////////////////////////////////
char term_buffer[max_chars];
int num_char;
int GetSerialBuffer(struct pt *pt)
{
    static char character;
    // mark the beginnning of the input thread
    PT_BEGIN(pt);
    num_char = 0;
    //memset(term_buffer, 0, max_chars);

    while(num_char < max_chars)
    {
        // get the character
        // yield until there is a valid character so that other
        // threads can execute
        PT_YIELD_UNTIL(pt, UARTReceivedDataIsAvailable(UART2));
       // while(!UARTReceivedDataIsAvailable(UART2)){};
        character = UARTGetDataByte(UART2);
        UARTSendDataByte(UART2, character);

        // unomment to check backspace character!!!
        //printf("--%x--",character );

        // end line
        if(character == '\r'){
            term_buffer[num_char] = 0; // zero terminate the string
            crlf; // send a new line
            break;
        }
        // backspace
        else if (character == backspace){
            putchar(' '); // write a blank over the previous character
            putchar(backspace); // go back one position
            num_char--;
            // check for buffer underflow
            if (num_char<0) {num_char = 0 ;}
        }
        else  {term_buffer[num_char++] = character ;} 
         //if (character == backspace)

    } //end while(num_char < max_size)

    // kill this input thread, to allow spawning thread to execute
    PT_EXIT(pt);
    // and indicate the end of the thread
    PT_END(pt);
}

// === Thread 1 ======================================================
/**
 * The first protothread function. A protothread function must always
 * return an integer, but must never explicitly return - returning is
 * performed inside the protothread statements.
 *
 * The protothread function is driven by the main loop further down in
 * the code.
 */
static PT_THREAD (protothread1(struct pt *pt))
{
    # define wait_t1 1000 // mSec
   
    // mark beginning of thread
    PT_BEGIN(pt);

    /* We loop forever here. */
    while(1) {
        //stop until thread 2 signals
        PT_SEM_WAIT(pt, &control_t1);

        printf("pt1 %d %ld\n", milliSec, uSec());

       // tell thread 2 to go
        PT_SEM_SIGNAL(pt, &control_t2);

        // This is a locally written macro using the timer ISR
        // to program a yield time
       PT_YIELD_TIME(wait_t1) ;

        // never exit while
  } // END WHILE(1)

  // mark end the thread
  PT_END(pt);
} // thread 1

// === Thread 2 ======================================================
//
static PT_THREAD (protothread2(struct pt *pt))
{
    PT_BEGIN(pt);

      while(1) {
            //stop until thread 1 signals
            PT_SEM_WAIT(pt, &control_t2);

            printf("pt2 %d\n", milliSec);

            // tell thread 1 to go
            PT_SEM_SIGNAL(pt, &control_t1);

            // never exit while
      } // END WHILE(1)
  PT_END(pt);
} // thread 2

// === Thread 3 ======================================================
//
static PT_THREAD (protothread3(struct pt *pt))
{
    PT_BEGIN(pt);
      while(1) {
            printf("pt3 %d\n", milliSec);

            //spawn a thread to handle terminal input
            // the input thread waits for input
            // -- BUT does NOT block other threads
            PT_SPAWN(pt, &pt_input, GetSerialBuffer(&pt_input) );
            // returns when the thead dies
            // in this case, when <enter> is pushed
            // now print the string
            printf("%s %d \n", term_buffer, num_char);

            // never exit while
      } // END WHILE(1)
  PT_END(pt);
} // thread 3

// === Main  ======================================================
// set up UART, timer2, threads
// then schedule them as fast as possible

int main(void)
{
  // === init the USART i/o pins =========
  PPSInput (2, U2RX, RPB11); //Assign U2RX to pin RPB11 -- Physical pin 22 on 28 PDIP
  PPSOutput(4, RPB10, U2TX); //Assign U2TX to pin RPB10 -- Physical pin 21 on 28 PDIP

  ANSELA =0; //make sure analog is cleared
  ANSELB =0;

  // === init the uart2 ===================
  UARTConfigure(UART2, UART_ENABLE_PINS_TX_RX_ONLY);
  UARTSetLineControl(UART2, UART_DATA_SIZE_8_BITS | UART_PARITY_NONE | UART_STOP_BITS_1);
  UARTSetDataRate(UART2, PB_FREQ, BAUDRATE);
  UARTEnable(UART2, UART_ENABLE_FLAGS(UART_PERIPHERAL | UART_RX | UART_TX));
  printf("protothreads start..\n\r");

  // ===Set up timer2 ======================
  // timer 2: on,  interrupts, internal clock, prescalar 1, toggle rate
  // run at 30000 ticks is 1 mSec
  OpenTimer2(T2_ON | T2_SOURCE_INT | T2_PS_1_1, 30000);
  // set up the timer interrupt with a priority of 2
  ConfigIntTimer2(T2_INT_ON | T2_INT_PRIOR_2);
  mT2ClearIntFlag(); // and clear the interrupt flag
  // init system time variable
  milliSec = 0;
  // setup system wide interrupts  
  INTEnableSystemMultiVectoredInt();

  // === now the threads ====================
  // init  the thread control semaphores
  PT_SEM_INIT(&control_t1, 0); // start blocked
  PT_SEM_INIT(&control_t2, 1); // start unblocked

  // init the threads
  PT_INIT(&pt1);
  PT_INIT(&pt2);
  PT_INIT(&pt3);
  
  // schedule the threads
  while(1) {
    PT_SCHEDULE(protothread1(&pt1));
    PT_SCHEDULE(protothread2(&pt2));
    PT_SCHEDULE(protothread3(&pt3));
  }
} // main