/**
 * 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

// semaphores for controlling threads
static struct pt_sem control_t1, control_t2 ;

// 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 ;
}

// === 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("P-thread 1 %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("P-thread 2 %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))
{
    # define wait_t3 4000 // mSec
    
    PT_BEGIN(pt);

      while(1) {
            printf("P-thread 3 %d\n", milliSec);

            // slow down!
            PT_YIELD_TIME(wait_t3);

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

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

static struct pt pt1, pt2, pt3;

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("pt 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