/**
 * Hunter Adams (vha3@cornell.edu)
 * 
 * HARDWARE CONNECTIONS
 *  - GPIO 16 ---> VGA Hsync
 *  - GPIO 17 ---> VGA Vsync
 *  - GPIO 18 ---> 330 ohm resistor ---> VGA Green lo-bit |__ both wired to 150 ohm to ground 
 *  - GPIO 19 ---> 220 ohm resistor ---> VGA Green hi_bit |   and to VGA Green
 *  - GPIO 20 ---> 330 ohm resistor ---> VGA Blue
 *  - GPIO 21 ---> 330 ohm resistor ---> VGA Red
 *  - RP2040 GND ---> VGA GND
 *
 * RESOURCES USED
 *  - PIO state machines 0, 1, and 2 on PIO instance 0
 *  - DMA channels 0, 1, 2, and 3
 *  - 153.6 kBytes of RAM (for pixel color data)
 *
 * Protothreads v1.1.1
 * Threads:
 * core 0:
 * Lattice Boltzmann
 * blink LED25 
 * core 1:
 * Toggle gpio 4 
 * Serial i/o 
 */
// ==========================================
// === VGA graphics library
// ==========================================
#include "vga16_graphics.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "pico/stdlib.h"
#include "hardware/pio.h"
#include "hardware/dma.h"
// // Our assembled programs:
// // Each gets the name <pio_filename.pio.h>
// #include "hsync.pio.h"
// #include "vsync.pio.h"
// #include "rgb.pio.h"

// ==========================================
// === protothreads globals
// ==========================================
#include "hardware/sync.h"
#include "hardware/timer.h"
#include "pico/multicore.h"
#include "string.h"
// protothreads header
#include "pt_cornell_rp2040_v1_1_1.h"

// ==========================================
// === fixed point 
// ==========================================
// s1.14 format
// == resolution 2^-14 = 1e-4
// == dynamic range is +1.9999/-2.0
/*
typedef signed short Fix ;
#define two30 1073741824.0
#define two14 16384
#define mFix(a,b) ((Fix)((((signed int)(a))*((signed int)(b)))>>14)) 
#define f2Fix(a) ((Fix)((a)*two14)) // 
#define Fix2f(a) ((float)(a)/two14)
#define absFix(a) abs(a) 
#define divFix(a,b) ((Fix)(((((signed int)(a))<<14)/(b)))) 
*/

/*
// s1.30 format
// == resolution 2^-30 = 1e-9
// == dynamic range is +1.9999/-2.0
typedef signed int Fix ;
#define two30 1073741824.0
#define two14 16384
#define mFix(a,b) ((Fix)((((signed long long)(a))*((signed long long)(b)))>>30)) 
#define f2Fix(a) ((Fix)((a)*two30)) // 
#define Fix2f(a) ((float)(a)/two30)
#define absFix(a) abs(a) 
#define divFix(a,b) ((Fix)(((((signed long long)(a))<<30)/(b)))) 
*/

// s1.25 format (FPGA size)
// == resolution 2^-25 
// == dynamic range is +1.9999/-2.0
typedef signed int Fix ;
#define two30 1073741824.0
#define two14 16384.0
#define two25 33554432.0
#define mFix(a,b) ((Fix)((((signed long long)(a))*((signed long long)(b)))>>25)) 
#define f2Fix(a) ((Fix)((a)*two25)) // 
#define Fix2f(a) ((float)(a)/two25)
#define absFix(a) abs(a) 
#define divFix(a,b) ((Fix)(((((signed long long)(a))<<25)/(b)))) 

// some multiply macros for small whole numbers
#define m3(a) ((a)+((a)<<1))
#define m1_5(a) ((a)+((a)>>1))
#define m4_5(a) (((a)<<2)+((a)>>1))
#define min(X,Y) ((X) < (Y) ? (X) : (Y))
#define max(X,Y) ((X) > (Y) ? (X) : (Y))

// ==================================================
// === Lattice Boltzmann code from Daniel V. Schroeder
// ==================================================
/*
A lattice-Boltzmann fluid simulation in JavaScript, using HTML5 canvas for graphics	
	Copyright 2013, Daniel V. Schroeder
  >>> Modifed for C on  Pi Pico by Bruce Land 2022 <<<
	Permission is hereby granted, free of charge, to any person obtaining a copy of 
	this software and associated data and documentation (the "Software"), to deal in 
	the Software without restriction, including without limitation the rights to 
	use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 
	of the Software, and to permit persons to whom the Software is furnished to do 
	so, subject to the following conditions:

	The above copyright notice and this permission notice shall be included in all 
	copies or substantial portions of the Software.

	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 
	INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
	PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR 
	ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 
	OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR 
	OTHER DEALINGS IN THE SOFTWARE.

	Except as contained in this notice, the name of the author shall not be used in 
	advertising or otherwise to promote the sale, use or other dealings in this 
	Software without prior written authorization.
  */
 // Global floatiables:	

	//int pxPerSquare = 2 ;
													// width of plotted grid site in pixels
	#define xdim  110			// grid dimensions for simulation 70x30 works also 100x20
	#define ydim 30 
	
	char tracerCheck = true ;
	char running = true;						// will be true when running
	int stepCount = 0;
	int startTime = 0;
	Fix four9ths = f2Fix(4.0 / 9.0) ;					// abbreviations
	Fix one9th = f2Fix(1.0 / 9.0) ;
	Fix one36th = f2Fix(1.0 / 36.0) ;
    Fix onefix = f2Fix(1.0) ;
    Fix halffix = f2Fix(0.5) ;
	Fix rho_twiddle = f2Fix(0.97) ;
	int barrierCount = 0;
	int barrierxSum = 0;
	int barrierySum = 0;
	float barrierFx = 0.0;						// total force on all barrier sites
	float barrierFy = 0.0;
	int time = 0;								// time (in simulation step units) since data collection started
	char showingPeriod = false;
	
	// Create the arrays of fluid particle densities, etc. (using 1D arrays for speed):
	// To index into these arrays, use x + y*xdim, traversing rows first and then columns.
	Fix n0 [xdim*ydim];			// microscopic densities along each lattice direction
	Fix nN [xdim*ydim];
	Fix nS [xdim*ydim];
	Fix nE [xdim*ydim];
	Fix nW [xdim*ydim];
	Fix nNE [xdim*ydim];
	Fix nSE [xdim*ydim];
	Fix nNW [xdim*ydim];
	Fix nSW [xdim*ydim];
	Fix rho [xdim*ydim];			// macroscopic density
	Fix ux  [xdim*ydim];			// macroscopic x-velocity
	Fix uy  [xdim*ydim];          // macroscopic y-velocity
	//Fix curl  [xdim*ydim];
	char barrier  [xdim*ydim];		// boolean array of barrier locations

  //#define nColors 16;

  // Initialize tracers (but don't place them yet):
	//#define nTracers  144
	//Fix tracerX [nTracers];
	//Fix tracerY [nTracers];
  
  // unstable above 0.10 or so
  Fix fluid_speed = f2Fix(0.11) ;
  // 30 bit is unstable at viscosity 0.002 and speed 0.10
  Fix fluid_viscosity = f2Fix(0.007) ;
  #define stepsPerFrame 1

// =========================================
// LB init function
// =========================================
void init_LB(void){
	// Initialize to a steady rightward flow with no barriers:
	for (int y=0; y<ydim; y++) {
		for (int x=0; x<xdim; x++) {
			barrier[x+y*xdim] = false;
		}
	}

  	//for (int t=0; t<nTracers; t++) {
	//	tracerX[t] = 0; tracerY[t] = 0;
	//}
} // end init_LB

// Set all densities in a cell to their equilibrium values for a given velocity and density:
// (If density is omitted, it's left unchanged.)
void setEquil(int x, int y, Fix newux, Fix newuy, Fix newrho) {
	int i = x + y*xdim;
	//
	Fix ux3 = m3(newux) ;// 3 * newux;
	Fix uy3 = m3(newuy) ;// 3 * newuy;
	Fix ux2 = mFix(newux, newux) ;
	Fix uy2 = mFix(newuy, newuy);
	Fix uxuy2 = mFix(newux, newuy)<<1 ; //2 * newux * newuy;
	Fix u2 = ux2 + uy2;
	Fix u215 = m1_5(u2) ; // 1.5 * u2;
    Fix one9th_newrho = mFix(one9th, newrho) ;
    Fix one36th_newrho = mFix(one36th, newrho) ;
    Fix four9ths_newrho = mFix(four9ths, newrho) ;
	//n0[i]  = mFix(four9ths_newrho, (1 - u215));
	nE[i]  = mFix(one9th_newrho,  (onefix + ux3       + m4_5(ux2)      - u215)) ;
	nW[i]  = mFix( one9th_newrho, (onefix - ux3       + m4_5(ux2)      - u215)) ;
	nN[i]  = mFix( one9th_newrho, (onefix + uy3       + m4_5(uy2)      - u215)) ;
	nS[i]  = mFix( one9th_newrho, (onefix - uy3       + m4_5(uy2)      - u215)) ;
	nNE[i] = mFix(one36th_newrho, (onefix + ux3 + uy3 + m4_5(u2+uxuy2) - u215)) ;
	nSE[i] = mFix(one36th_newrho, (onefix + ux3 - uy3 + m4_5(u2-uxuy2) - u215)) ;
	nNW[i] = mFix(one36th_newrho, (onefix - ux3 + uy3 + m4_5(u2-uxuy2) - u215)) ;
	nSW[i] = mFix(one36th_newrho, (onefix - ux3 - uy3 + m4_5(u2+uxuy2) - u215)) ;
    n0[i] = newrho - (nE[i]+nW[i]+nN[i]+nS[i]+nNE[i]+nSE[i]+nNW[i]+nSW[i]) ;
	rho[i] = newrho;
	ux[i] = newux;
	uy[i] = newuy;
}

// Function to initialize or re-initialize the fluid, based on speed slider setting:
void initFluid() {
	// Amazingly, if I nest the y loop inside the x loop, Firefox slows down by a factor of 20
	Fix u0 = fluid_speed;
	for (int y=0; y<ydim; y++) {
		for (int x=0; x<xdim; x++) {
			setEquil(x, y, u0, 0, onefix);
            //setEquil(x, y, u0, 0, 1);
			//curl[x+y*xdim] = 0.0;
		}
	}
//paintCanvas();
} // end initFluid

// Set the fluid floatiables at the boundaries, according to the current slider value:
void setBoundaries() {
	Fix u0 = fluid_speed;
	for (int x=0; x<xdim; x++) {
		setEquil(x, 0, u0, 0, onefix);
		setEquil(x, ydim-1, u0, 0, onefix);
	}
	for (int y=1; y<ydim-1; y++) {
		setEquil(0, y, u0, 0, onefix);
		setEquil(xdim-1, y, u0, 0, onefix);
	}
} // end setBoundaries()

// Collide particles within each cell (here's the physics!):
void collide(void) {
	Fix viscosity = fluid_viscosity;	// kinematic viscosity coefficient in natural units
	Fix omega = divFix(onefix, (m3(viscosity) + halffix));		// reciprocal of relaxation time
	// use these to split between the two processors
	#define xbottom 1
	#define xtop xdim
	for (int y=1; y<ydim-1; y++) {
		for (int x=xbottom; x<xtop-1; x++) {
			int i = x + y*xdim;		// array index for this lattice site
			Fix thisrho = n0[i] + nN[i] + nS[i] + nE[i] + nW[i] + nNW[i] + nNE[i] + nSW[i] + nSE[i];
			rho[i] = thisrho;
			Fix rho_inv = onefix - (thisrho - onefix) ;
			//Fix thisux = divFix((nE[i] + nNE[i] + nSE[i] - nW[i] - nNW[i] - nSW[i]), thisrho);
			Fix thisux = mFix((nE[i] + nNE[i] + nSE[i] - nW[i] - nNW[i] - nSW[i]), rho_inv);
			ux[i] = thisux;
			//Fix thisuy = divFix((nN[i] + nNE[i] + nNW[i] - nS[i] - nSE[i] - nSW[i]), thisrho);
			Fix thisuy = mFix((nN[i] + nNE[i] + nNW[i] - nS[i] - nSE[i] - nSW[i]), rho_inv);
			uy[i] = thisuy ;
			Fix one9thrho = mFix(one9th, thisrho);		// pre-compute a bunch of stuff for optimization
			Fix one36thrho = mFix(one36th, thisrho);
			Fix ux3 = m3(thisux);
			Fix uy3 = m3(thisuy);
			Fix ux2 = mFix(thisux, thisux);
			Fix uy2 = mFix(thisuy, thisuy);
			Fix uxuy2 = mFix(thisux, thisuy)<<1;
			Fix u2 = ux2 + uy2;
			Fix u215 = m1_5(u2);
            
			//n0[i]  += omega * (four9ths*thisrho * (1                        - u215) - n0[i]);
			nE[i]  += mFix(omega, ( mFix(one9thrho, (onefix + ux3    + m4_5(ux2)      - u215)) - nE[i]));
			nW[i]  += mFix(omega, ( mFix(one9thrho, (onefix - ux3    + m4_5(ux2)      - u215)) - nW[i]));
			nN[i]  += mFix(omega, ( mFix(one9thrho, (onefix + uy3    + m4_5(uy2)      - u215)) - nN[i]));
			nS[i]  += mFix(omega, ( mFix(one9thrho, (onefix - uy3    + m4_5(uy2)      - u215)) - nS[i]));
			nNE[i] += mFix(omega, ( mFix(one36thrho, (onefix + ux3 + uy3 + m4_5(u2+uxuy2) - u215)) - nNE[i]));
			nSE[i] += mFix(omega, ( mFix(one36thrho, (onefix + ux3 - uy3 + m4_5(u2-uxuy2) - u215)) - nSE[i]));
			nNW[i] += mFix(omega, ( mFix(one36thrho, (onefix - ux3 + uy3 + m4_5(u2-uxuy2) - u215)) - nNW[i]));
			nSW[i] += mFix(omega, ( mFix(one36thrho, (onefix - ux3 - uy3 + m4_5(u2+uxuy2) - u215)) - nSW[i]));
			// force conservation of mass for limited precision aithmetic
			n0[i]   = thisrho - (nE[i]+nW[i]+nN[i]+nS[i]+nNE[i]+nSE[i]+nNW[i]+nSW[i]);
		}
	}
	for (int y=1; y<ydim-2; y++) {
		nW[xdim-1+y*xdim] = nW[xdim-2+y*xdim];		// at right end, copy left-flowing densities from next row to the left
		nNW[xdim-1+y*xdim] = nNW[xdim-2+y*xdim];
		nSW[xdim-1+y*xdim] = nSW[xdim-2+y*xdim];
	}
} // end collide

// Move particles along their directions of motion:
void stream(void) {
	barrierCount = 0; barrierxSum = 0; barrierySum = 0;
	barrierFx = 0.0; barrierFy = 0.0;
	for (int y=ydim-2; y>0; y--) {			// first start in NW corner...
		for (int x=1; x<xdim-1; x++) {
			nN[x+y*xdim] = nN[x+(y-1)*xdim];			// move the north-moving particles
			nNW[x+y*xdim] = nNW[x+1+(y-1)*xdim];		// and the northwest-moving particles
		}
	}
	for (int y=ydim-2; y>0; y--) {			// now start in NE corner...
		for (int x=xdim-2; x>0; x--) {
			nE[x+y*xdim] = nE[x-1+y*xdim];			// move the east-moving particles
			nNE[x+y*xdim] = nNE[x-1+(y-1)*xdim];		// and the northeast-moving particles
		}
	}
	for (int y=1; y<ydim-1; y++) {			// now start in SE corner...
		for (int x=xdim-2; x>0; x--) {
			nS[x+y*xdim] = nS[x+(y+1)*xdim];			// move the south-moving particles
			nSE[x+y*xdim] = nSE[x-1+(y+1)*xdim];		// and the southeast-moving particles
		}
	}
	for (int y=1; y<ydim-1; y++) {				// now start in the SW corner...
		for (int x=1; x<xdim-1; x++) {
			nW[x+y*xdim] = nW[x+1+y*xdim];			// move the west-moving particles
			nSW[x+y*xdim] = nSW[x+1+(y+1)*xdim];		// and the southwest-moving particles
		}
	}
	for (int y=1; y<ydim-1; y++) {				// Now handle bounce-back from barriers
		for (int x=1; x<xdim-1; x++) {
			if (barrier[x+y*xdim]) {
				int index = x + y*xdim;
				nE[x+1+y*xdim] = nW[index];
				nW[x-1+y*xdim] = nE[index];
				nN[x+(y+1)*xdim] = nS[index];
				nS[x+(y-1)*xdim] = nN[index];
				nNE[x+1+(y+1)*xdim] = nSW[index];
				nNW[x-1+(y+1)*xdim] = nSE[index];
				nSE[x+1+(y-1)*xdim] = nNW[index];
				nSW[x-1+(y-1)*xdim] = nNE[index];
			}
		}
	}
} // end stream

/*
// Move the tracer particles:
void moveTracers(void) {
	for (int t=0; t<nTracers; t++) {
		//float roundedX = Math.round();
		//float roundedY = Math.round();
		int index = tracerX[t] + tracerY[t]*xdim;
		tracerX[t] += ux[index];
		tracerY[t] += uy[index];
		if (tracerX[t] > xdim-1) {
			tracerX[t] = 0;
			//tracerY[t] = Math.random() * ydim;
		}
	}
}
*/

// Simulate function executes a bunch of steps and then schedules another call to itself:
void simulate(void) {
	//
	setBoundaries();	
	// Execute a bunch of time steps:
	for (int step=0; step<stepsPerFrame; step++) {
		collide();
		stream();
		//moveTracers();
	}

	if (running) {
		stepCount += stepsPerFrame;
	}
	//char stable = true;
	//for (int x=0; x<xdim; x++) {
	//	int index = x + (ydim/2)*xdim;	// look at middle row only
	//	if (rho[index] <= 0) stable = false;
	//}//
	//if (!stable) {
		//window.alert("The simulation has become unstable due to excessive fluid speeds.");
	//	startStop();
	//	initFluid();
	//}	
}

	/*
	// Initialize the tracer particles:
	function initTracers() {
		if (tracerCheck.checked) {
			float nRows = Math.ceil(Math.sqrt(nTracers));
			float dx = xdim / nRows;
			float dy = ydim / nRows;
			float nextX = dx / 2;
			float nextY = dy / 2;
			for (float t=0; t<nTracers; t++) {
				tracerX[t] = nextX;
				tracerY[t] = nextY;
				nextX += dx;
				if (nextX > xdim) {
					nextX = dx / 2;
					nextY += dy;
				}
			}
		}
	}
*/



/*
// Compute the curl (actually times 2) of the macroscopic velocity field, for plotting:
void computeCurl() {
	for (int y=1; y<ydim-1; y++) {			// interior sites only; leave edges set to zero
		for (int x=1; x<xdim-1; x++) {
			curl[x+y*xdim] = uy[x+1+y*xdim] - uy[x-1+y*xdim] - ux[x+(y+1)*xdim] + ux[x+(y-1)*xdim];
		}
	}
}
*/
	

/*
	// Draw the tracer particles:
	function drawTracers() {
		context.fillStyle = "rgb(150,150,150)";
		for (float t=0; t<nTracers; t++) {
			float canvasX = (tracerX[t]+0.5) * pxPerSquare;
			float canvasY = canvas.height - (tracerY[t]+0.5) * pxPerSquare;
			context.fillRect(canvasX-1, canvasY-1, 2, 2);
		}
	}
*/

	// Add a barrier at a given grid coordinate location:
	void addBarrier(int x, int y) {
		if ((x > 1) && (x < xdim-2) && (y > 1) && (y < ydim-2)) {
			barrier[x+y*xdim] = true;
		}
	}

	// Remove a barrier at a given grid coordinate location:
	void removeBarrier(int x, int y) {
		if (barrier[x+y*xdim]) {
			barrier[x+y*xdim] = false;
		}
	}

	// Clear all barriers:
	void clearBarriers(void) {
		for (int x=0; x<xdim; x++) {
			for (int y=0; y<ydim; y++) {
				barrier[x+y*xdim] = false;
			}
		}
	}

	

// ==================================================
// === Lattice Boltzmann demo -- RUNNING on core 0
// ==================================================


static PT_THREAD (protothread_graphics(struct pt *pt)) {
    PT_BEGIN(pt);
    // the protothreads interval timer
    PT_INTERVAL_INIT() ;

    // Draw some filled rectangles
    fillRect(64, 0, 176, 50, BLUE); // blue box
    //fillRect(250, 0, 176, 50, ORANGE); // red box
   //fillRect(435, 0, 176, 50, GREEN); // green box

    // Write some text
    setTextColor(WHITE) ;
    setCursor(65, 0) ;
    setTextSize(1) ;
    writeString("Raspberry Pi Pico") ;
    setCursor(65, 10) ;
    writeString("Lattice Boltzmann 110x30") ;
    setCursor(65, 20) ;
    writeString("Hunter Adams / Bruce Land") ;
    setCursor(65, 30) ;
    writeString("vha3@cornell.edu") ;
	/*
    setCursor(445, 10) ;
    setTextColor(BLACK) ;
    setTextSize(1) ;
    writeString("Protothreads rp2040 v1.1.1") ;
    setCursor(445, 20) ;
    writeString("Mod for 16 colors (brl4)") ;
    setTextColor(WHITE) ;
    */

    char video_buffer[32];

    // the Lattice-boltzmaann setup
    init_LB();
    // initialize to steady rightward flow
    initFluid();		
    // put in a vertical barrier
	// about 1/3 of width
	for (int y=ydim/2-3; y<ydim/2+4; y++) {
		addBarrier(10, y) ;
	}
    // 

    while(true) {
      // A brief nap
      PT_YIELD(pt) ;

      // compute one time iteration 
      int elapsed_time = PT_GET_TIME_usec();

      // main simulation loop 
	  setBoundaries();	
	  // Execute update 
	  collide();
	  stream();
	  //moveTracers();
	  //setEquil(30, ydim-1, 0, -fluid_speed, onefix);
	  //computeCurl() ;

	  Fix rhomax = 0 ;
	  for (int x=0; x<xdim; x++) {
			for (int y=0; y<ydim; y++) {
                int i = x+y*xdim ;
				//if (rho[i]>rhomax) rhomax = rho[i] ;
				//short speed = 50*sqrt(ux[x+y*xdim]*ux[x+y*xdim]+uy[x+y*xdim]*uy[x+y*xdim]);
                // for s1x14 shift 7
                // for s1x30 shift 24
                // for s1x25 shift 19
                short speed = (max(abs(ux[i]), abs(uy[i])) + (min(abs(ux[i]), abs(uy[i])) >>1) >> 19) ;
				drawPixel(x+65, y+70, speed) ;

				//short color = curl[i]>>20 ;
				//drawPixel(x+65, y+70, color) ;
				
			}
		}

      elapsed_time = PT_GET_TIME_usec()-elapsed_time ;
      sprintf(video_buffer, "T = %d mSec ", elapsed_time/1000);
      setCursor(65, 40) ;
      setTextColor2(BLACK, BLUE) ;
      setTextSize(1) ;
      writeString(video_buffer) ;
//
   }

   PT_END(pt);
} // graphics thread

// ==================================================
// === toggle25 thread on core 0
// ==================================================
// the on-board LED blinks
static PT_THREAD (protothread_toggle25(struct pt *pt))
{
    PT_BEGIN(pt);
    static bool LED_state = false ;
    
     // set up LED p25 to blink
     gpio_init(25) ;	
     gpio_set_dir(25, GPIO_OUT) ;
     gpio_put(25, true);
     // data structure for interval timer
     PT_INTERVAL_INIT() ;

      while(1) {
        // yield time 0.1 second
        //PT_YIELD_usec(100000) ;
        PT_YIELD_INTERVAL(100000) ;

        // toggle the LED on PICO
        LED_state = LED_state? false : true ;
        gpio_put(25, LED_state);
        //
        // NEVER exit while
      } // END WHILE(1)
  PT_END(pt);
} // blink thread


// ==================================================
// === toggle gpio 4 thread -- RUNNING on core 1
// ==================================================
// toggle gpio 4 
static PT_THREAD (protothread_toggle_gpio4(struct pt *pt))
{
    PT_BEGIN(pt);
    static bool LED_state = false ;
    //
     // set up LED gpio 4 to blink
     gpio_init(4) ;	
     gpio_set_dir(4, GPIO_OUT) ;
     gpio_put(4, true);
     // data structure for interval timer
     PT_INTERVAL_INIT() ;

      while(1) {
        //
        PT_YIELD_INTERVAL(20) ;
        // toggle gpio 4
        LED_state = !LED_state ;
        gpio_put(4, LED_state);
        //
        // NEVER exit while
      } // END WHILE(1)
  PT_END(pt);
} // blink thread

// ==================================================
// === user's serial input thread on core 1
// ==================================================
// serial_read an serial_write do not block any thread
// except this one
static PT_THREAD (protothread_serial(struct pt *pt))
{
    PT_BEGIN(pt);
      static int test_in1, test_in2, sum ;
      //
      while(1) {
        // print prompt
        sprintf(pt_serial_out_buffer, "input x y: ");
        // spawn a thread to do the non-blocking write
        serial_write ;

        // spawn a thread to do the non-blocking serial read
         serial_read ;
        
        // convert input string to number
        //sscanf(pt_serial_in_buffer,"%d %d ",  &col1, &col2) ;
        //printf("color=%d\n\r", readPixel(col1, col2)) ;

        // NEVER exit while
      } // END WHILE(1)
  PT_END(pt);
} // serial thread

// ========================================
// === core 1 main -- started in main below
// ========================================
void core1_main(){ 
  //
  //  === add threads  ====================
  // for core 1
  pt_add_thread(protothread_toggle_gpio4) ;
  //pt_add_thread(protothread_serial) ;
  //
  // === initalize the scheduler ==========
  pt_schedule_start ;
  // NEVER exits
  // ======================================
}

// ========================================
// === core 0 main
// ========================================
int main(){
  // set the clock
  //set_sys_clock_khz(250000, true); // 171us
  // start the serial i/o
  stdio_init_all() ;
  // announce the threader version on system reset
  printf("\n\rProtothreads RP2040 v1.11 two-core\n\r");

  // Initialize the VGA screen
  initVGA() ;
     
  // start core 1 threads
  multicore_reset_core1();
  multicore_launch_core1(&core1_main);

  // === config threads ========================
  // for core 0
  pt_add_thread(protothread_graphics);
  pt_add_thread(protothread_toggle25);
  //
  // === initalize the scheduler ===============
  pt_schedule_start ;
  // NEVER exits
  // ===========================================
} // end main