diff --git a/README.adoc b/README.adoc
index aef62e8..62af6dd 100644
--- a/README.adoc
+++ b/README.adoc
@@ -1,6 +1,6 @@
 = Scheduler Library for Arduino =
 
-The Scheduler library enables the Arduino Due to run multiple functions at the same time. This allows tasks to happen without interrupting each other. 
+The Scheduler library enables the Arduino to run multiple functions at the same time. This allows tasks to happen without interrupting each other. 
 
 For more information about this library please visit us at
 http://www.arduino.cc/en/Reference/Scheduler
diff --git a/examples/MultiplePWM/Loop2.ino b/examples/MultiplePWM/Loop2.ino
new file mode 100644
index 0000000..e503cc5
--- /dev/null
+++ b/examples/MultiplePWM/Loop2.ino
@@ -0,0 +1,8 @@
+
+// Task no.2
+
+void loop2 () {
+ analogWrite(LED2, counter1);
+ counter1 += 50;
+ delay(500);
+}
diff --git a/examples/MultiplePWM/Loop3.ino b/examples/MultiplePWM/Loop3.ino
new file mode 100644
index 0000000..64bb478
--- /dev/null
+++ b/examples/MultiplePWM/Loop3.ino
@@ -0,0 +1,8 @@
+
+// Task no.3
+
+void loop3 () {
+ analogWrite(LED3, counter2);
+ counter2++;
+ delay(7);
+}
diff --git a/examples/MultiplePWM/MultiplePWM.ino b/examples/MultiplePWM/MultiplePWM.ino
new file mode 100644
index 0000000..ca2c84b
--- /dev/null
+++ b/examples/MultiplePWM/MultiplePWM.ino
@@ -0,0 +1,54 @@
+/*
+ Multiple PWM
+
+ Demonstrates the use of the Scheduler library
+
+ Hardware required :
+ * LEDs connected to PWM pins
+
+ created 28 Dic 2015
+ by Testato
+ 
+*/
+
+
+// Include Scheduler since we want to manage multiple tasks.
+#include <Scheduler.h>
+
+#define LED1 13
+#define LED2 10
+#define LED3 11
+byte counter1;
+byte counter2;
+
+
+void setup() {
+
+  // Setup the led pins as OUTPUT
+  pinMode(LED1, OUTPUT);
+  pinMode(LED2, OUTPUT);
+  pinMode(LED3, OUTPUT);
+
+  // Add "loop2 and loop3" to scheduler
+  // "loop" is always started by default
+  Scheduler.startLoop(loop2);
+  Scheduler.startLoop(loop3);
+}
+
+
+// Task no.1 (standard Arduino loop() )
+void loop() {
+  digitalWrite(LED1, HIGH);
+  delay(1000);
+  digitalWrite(LED1, LOW);
+  delay(1000);
+  
+  // When multiple tasks are running, 'delay' passes control to
+  // other tasks while waiting and guarantees they get executed
+  // It is not necessary to call yield() when using delay() 
+}
+
+
+
+
+
diff --git a/examples/MultipleSketch/MultipleSketch.ino b/examples/MultipleSketch/MultipleSketch.ino
new file mode 100644
index 0000000..94429df
--- /dev/null
+++ b/examples/MultipleSketch/MultipleSketch.ino
@@ -0,0 +1,51 @@
+/*
+ Multiple Sketch
+
+ Demonstrates the use of .start and .startLoop method of the Scheduler library
+
+ Hardware required :
+ * LEDs connected to pins
+
+ created 07 Gen 2016
+ by Testato
+ 
+*/
+
+
+// Include Scheduler since we want to manage multiple tasks.
+#include <Scheduler.h>
+
+#define LED1 13
+#define LED2 10
+#define LED3 11
+
+
+// Sketch no.1 
+
+void setup() {
+  // Setup the led pin as OUTPUT
+  pinMode(LED1, OUTPUT);
+
+  // Add "setup2 and setup3" to scheduler
+  Scheduler.start(setup2);
+  Scheduler.start(setup3);
+  // Add "loop2 and loop3" to scheduler
+  Scheduler.startLoop(loop2);
+  Scheduler.startLoop(loop3);
+}
+
+
+void loop() {
+  digitalWrite(LED1, HIGH);
+  delay(1000);
+  digitalWrite(LED1, LOW);
+  delay(1000);
+  
+  // When multiple tasks are running, 'delay' passes control to
+  // other tasks while waiting and guarantees they get executed
+  // It is not necessary to call yield() when using delay() 
+}
+
+
+
+
diff --git a/examples/MultipleSketch/Sketch2 b/examples/MultipleSketch/Sketch2
new file mode 100644
index 0000000..1a2d854
--- /dev/null
+++ b/examples/MultipleSketch/Sketch2
@@ -0,0 +1,13 @@
+
+// Sketch no.2
+
+void setup2 () {
+    pinMode(LED2, OUTPUT);
+}
+
+void loop2 () {
+  digitalWrite(LED2, HIGH);
+  delay(500);
+  digitalWrite(LED2, LOW);
+  delay(500);
+}
diff --git a/examples/MultipleSketch/Sketch3 b/examples/MultipleSketch/Sketch3
new file mode 100644
index 0000000..f407c19
--- /dev/null
+++ b/examples/MultipleSketch/Sketch3
@@ -0,0 +1,13 @@
+
+// Sketch no.3
+
+void setup3 () {
+    pinMode(LED3, OUTPUT);
+}
+
+void loop3 () {
+  digitalWrite(LED3, HIGH);
+  delay(250);
+  digitalWrite(LED3, LOW);
+  delay(250);
+}
diff --git a/examples/SchedulerMemoryDump/SchedulerMemoryDump.ino b/examples/SchedulerMemoryDump/SchedulerMemoryDump.ino
new file mode 100644
index 0000000..c1104a8
--- /dev/null
+++ b/examples/SchedulerMemoryDump/SchedulerMemoryDump.ino
@@ -0,0 +1,167 @@
+/*
+ Scheduler View Memory Organization
+
+ Demonstrates to view and debug task memory
+
+ created 21 gen 2016
+*/
+
+// Include Scheduler since we want to manage multiple tasks.
+#include <Scheduler.h>
+
+// loop2 static memory
+// set memory size
+const int taskMemory = DEFAULT_STACK_SIZE;
+// Create memory for loop2
+TaskContext taskContext2;
+TaskStack taskStack2[ taskMemory ];
+
+
+//vector of structure contiene dati di ogni processo, sp spend context heap start heapend.
+
+typedef enum {TYPE_SPEND, TYPE_SP, TYPE_CONTEXT, TYPE_CONTEXTEND, TYPE_HEAP, TYPE_HEAPEND, TYPE_COUNT} AddressType;
+
+struct MemoryDump {
+  uint32_t address;
+  AddressType type;
+  char name[7];
+} dump[14];
+byte curDump = 0;
+
+
+
+void setup() {
+  Serial.begin(9600);
+
+  // Add "loop2" in global memory and "loop3" in heap memory.
+  // "loop" is always started by default, use global memory for the context and is unico can be used the stack.
+  Scheduler.startLoop(loop2, taskMemory, taskStack2, taskContext2);
+  Scheduler.startLoop(loop3);
+}
+
+// Task no.1: passo subito l'esecuzione agli altri task che salveranno i dati
+void loop() {
+  yield();
+  
+  dump[curDump].address = Scheduler.stackPtr();
+  dump[curDump].type = TYPE_SP;
+  strcpy(dump[curDump].name, "loop");
+  ++curDump;
+  
+  dump[curDump].address = Scheduler.stackEnd();
+  dump[curDump].type = TYPE_SPEND;
+  strcpy(dump[curDump].name, "loop");
+  ++curDump;
+  
+  dump[curDump].address = Scheduler.contextPtr();
+  dump[curDump].type = TYPE_CONTEXT;
+  strcpy(dump[curDump].name, "loop");
+  ++curDump;
+
+  dump[curDump].address = Scheduler.contextPtr() + TASK_CONTEXT_SIZE;
+  dump[curDump].type = TYPE_CONTEXTEND;
+  strcpy(dump[curDump].name, "loop");
+  ++curDump;
+  
+  dump[curDump].address = Scheduler.heapStart();
+  dump[curDump].type = TYPE_HEAP;
+  strcpy(dump[curDump].name, "sram");
+  ++curDump;
+
+  dump[curDump].address = Scheduler.heapEnd();
+  dump[curDump].type = TYPE_HEAPEND;
+  strcpy(dump[curDump].name, "sram");
+  ++curDump;
+  
+  //riordino dati
+  struct MemoryDump temp;
+  byte swap;
+  do {
+    byte i;
+    for( swap = 0, i = 0; i < curDump - 1; ++i ) {
+      if ( dump[i].address > dump[i + 1].address ) {
+        temp = dump[i];
+        dump[i] = dump[i + 1];
+        dump[i + 1] = temp;
+        swap = 1;
+      }
+    }
+  }while(swap);
+
+  const char* memName[] = { "stack end", "stack pointer", "context start", "context end", "heap start", "heap end"};
+  char adr[8];
+  byte i;
+  
+  Serial.println(F("     --------------"));
+  Serial.println(F("     ---- SRAM ----"));
+  Serial.println(F("     --------------"));
+  Serial.println(F("     ______________"));  
+  for( swap = 0, i = 0; i < curDump; ++i ) {
+      Serial.print(F("     | "));
+      sprintf(adr,"0x%.8X",(unsigned)dump[i].address);
+      Serial.print(adr);
+      Serial.print(F(" |<--"));
+      Serial.print(memName[dump[i].type]);
+      Serial.print(" ");
+      Serial.println(dump[i].name);
+      if ( dump[i].type & 1 || dump[i].type == 4)
+        Serial.println(F("     |____________|"));
+      else
+        Serial.println(F("     |            |"));
+  }
+  Serial.println(F("     --------------"));
+  
+  while(1);
+}
+
+// Task no.2: save data
+void loop2() {
+  dump[curDump].address = Scheduler.stackPtr();
+  dump[curDump].type = TYPE_SP;
+  strcpy(dump[curDump].name, "loop2");
+  ++curDump;
+  
+  dump[curDump].address = Scheduler.stackEnd();
+  dump[curDump].type = TYPE_SPEND;
+  strcpy(dump[curDump].name, "loop2");
+  ++curDump;
+  
+  dump[curDump].address = Scheduler.contextPtr();
+  dump[curDump].type = TYPE_CONTEXT;
+  strcpy(dump[curDump].name, "loop2");
+  ++curDump;
+
+  dump[curDump].address = Scheduler.contextPtr() + TASK_CONTEXT_SIZE;
+  dump[curDump].type = TYPE_CONTEXTEND;
+  strcpy(dump[curDump].name, "loop2");
+  ++curDump;
+  
+  yield();
+  while(1);
+}
+
+// Task no.3:
+void loop3() {
+  dump[curDump].address = Scheduler.stackPtr();
+  dump[curDump].type = TYPE_SP;
+  strcpy(dump[curDump].name, "loop3");
+  ++curDump;
+  
+  dump[curDump].address = Scheduler.stackEnd();
+  dump[curDump].type = TYPE_SPEND;
+  strcpy(dump[curDump].name, "loop3");
+  ++curDump;
+  
+  dump[curDump].address = Scheduler.contextPtr();
+  dump[curDump].type = TYPE_CONTEXT;
+  strcpy(dump[curDump].name, "loop3");
+  ++curDump;
+
+  dump[curDump].address = Scheduler.contextPtr() + TASK_CONTEXT_SIZE;
+  dump[curDump].type = TYPE_CONTEXTEND;
+  strcpy(dump[curDump].name, "loop3");
+  ++curDump;
+  
+  yield();
+  while(1);
+}
diff --git a/examples/SchedulerPriority/SchedulerPriority.ino b/examples/SchedulerPriority/SchedulerPriority.ino
new file mode 100644
index 0000000..d7b43a2
--- /dev/null
+++ b/examples/SchedulerPriority/SchedulerPriority.ino
@@ -0,0 +1,80 @@
+/*
+ Scheduler Priority
+
+ Demonstrates the use of the priority
+
+ Hardware required :
+ Serial
+
+ Serial Monitor:
+ Send 'r' to reverse priority and send 'n' to reset priority
+
+ created 3 feb 2016
+*/
+
+
+// Include Scheduler since we want to manage multiple tasks.
+#include <Scheduler.h>
+
+TaskContext taskContext2;
+TaskContext taskContext3;
+TaskStack taskStack2[ DEFAULT_STACK_SIZE ];
+TaskStack taskStack3[ DEFAULT_STACK_SIZE ];
+tid_t loop2ID;
+tid_t loop3ID;
+
+void setup() {
+  Serial.begin(9600);
+
+  //Default priority loop == 7(max)
+  //Default priority Task == 0(min)
+
+  //First set priority and after start task
+  Scheduler.setPriority(5);
+  loop2ID = Scheduler.startLoop(loop2, sizeof taskStack2, taskStack2, taskContext2);
+  Scheduler.setPriority(3);
+  loop3ID = Scheduler.startLoop(loop3, sizeof taskStack3, taskStack3, taskContext3);  
+  //execution:
+  //loop(7) -> loop2(5) -> loop3(3)
+}
+
+void loop() {
+  Serial.println(F("loop"));
+  if ( Serial.available() ) {
+      switch ( Serial.read() ) {
+          case 'r':
+            //reverse priority
+            Scheduler.taskPriority(loop3ID, 7);
+            //0 is current task
+            Scheduler.taskPriority(0, 3);
+          break;
+
+          case 'n':
+            //normal priority
+            Scheduler.taskPriority(loop3ID, 3);
+            //0 is current task
+            Scheduler.taskPriority(0, 7);
+          break;
+      }
+      
+  }
+
+  //delay without yield
+  Scheduler.wait(800);
+  yield();
+}
+
+void loop2() {
+  Serial.println(F("loop2"));
+  //delay without yield
+  Scheduler.wait(800);
+  yield();
+}
+
+void loop3() {
+  Serial.println(F("loop3"));
+  Serial.println("");
+  //delay without yield
+  Scheduler.wait(800);
+  yield();
+}
diff --git a/examples/SchedulerResumeStop/SchedulerResumeStop.ino b/examples/SchedulerResumeStop/SchedulerResumeStop.ino
new file mode 100644
index 0000000..523e575
--- /dev/null
+++ b/examples/SchedulerResumeStop/SchedulerResumeStop.ino
@@ -0,0 +1,53 @@
+/*
+ Scheduler Resume Stop Task
+
+ Demonstrates to stop and resume task
+
+ Hardware required :
+ Serial
+
+ Send 1 to resume task, 0 to stop task.
+ 
+ created 2 fed 2016
+*/
+
+// Include Scheduler since we want to manage multiple tasks.
+#include <Scheduler.h>
+
+// loop2
+TaskContext taskContext2;
+TaskStack taskStack2[ DEFAULT_STACK_SIZE ];
+tid_t loop2ID;
+
+void setup() {
+  Serial.begin(9600);
+  pinMode(13, OUTPUT);
+  
+  //start loop2 and get task id
+  loop2ID = Scheduler.startLoop(loop2, DEFAULT_STACK_SIZE, taskStack2, taskContext2);
+}
+
+void loop() {
+  if ( Serial.available() ) {
+    char cmd = Serial.read();
+
+    switch ( cmd ) {
+      case '0':
+        Scheduler.taskStop(loop2ID);
+      break;
+
+      case '1':
+        Scheduler.taskResume(loop2ID);
+      break;
+    }
+  }
+  yield();
+}
+
+void loop2() {
+  digitalWrite(13, HIGH);
+  delay(500);
+  digitalWrite(13, LOW);
+  delay(500);
+}
+
diff --git a/examples/SchedulerServoWithoutTimer/SchedulerServoWithoutTimer.ino b/examples/SchedulerServoWithoutTimer/SchedulerServoWithoutTimer.ino
new file mode 100644
index 0000000..dbd7527
--- /dev/null
+++ b/examples/SchedulerServoWithoutTimer/SchedulerServoWithoutTimer.ino
@@ -0,0 +1,149 @@
+/*
+ Scheduler Servo without timer
+ Demonstrates to control servomotor without use timer
+
+ Hardware required :
+ Serial, Servo
+
+ Software:
+ Connect servo to pin 5 otherwise change SERVO_PIN
+ the default time servo is 10ms for 0°, 20ms for 180° and 200ms period, read your datasheet and change SERVO_MIN, SERVO_MAX and SERVO_PERIOD.
+ Serial monitor end of char '\n'
+
+ Usage:
+ Send 'a' for attach
+ Send 'd' for detach
+ Send value in range '0' ... '180' to move servo.
+ 
+
+ created 5 feb 2016
+*/
+
+// Include Scheduler since we want to manage multiple tasks.
+#include <Scheduler.h>
+
+
+/* GLOBAL FOR SERVO */
+//Task
+TaskContext servoContext;
+TaskStack servoStack[ DEFAULT_STACK_SIZE ];
+tid_t servoID;
+//degrees servo
+unsigned long servoTon  = 1500;
+unsigned long servoTime = 0;
+//pin servo
+#define SERVO_PIN         5
+#define SERVO_OFFSET_TIME 90
+#define SERVO_PERIOD      20000
+#define SERVO_MIN         1000
+#define SERVO_MAX         2000
+/********************/
+
+
+/* GLOBAL FOR REMOTE COMMAND */
+//task
+TaskContext cmdContext;
+TaskStack cmdStack[ DEFAULT_STACK_SIZE ];
+tid_t cmdID;
+/*******************************/
+
+
+/* FUNCTION SERVO */
+
+void servo_setup() {
+  servoID = Scheduler.startLoop(servo_loop, sizeof servoStack, servoStack, servoContext);
+  servoTon = 1500;
+  Scheduler.taskStop(servoID);
+}
+
+void servo_attach() {
+  pinMode(SERVO_PIN, OUTPUT);
+  digitalWrite(SERVO_PIN, HIGH);
+  servoTon = 1500;
+  servoTime = micros();
+  Scheduler.taskResume(servoID);
+}
+
+void servo_detach() {
+  digitalWrite(SERVO_PIN, LOW);
+  Scheduler.taskStop(servoID);
+}
+
+void servo_loop() {
+  while ( micros() - servoTime < servoTon - SERVO_OFFSET_TIME ) yield();
+  while ( micros() - servoTime < servoTon);
+  digitalWrite(SERVO_PIN, LOW);
+  
+  while ( micros() - servoTime < SERVO_PERIOD - SERVO_OFFSET_TIME ) yield();
+  while ( micros() - servoTime < SERVO_PERIOD);
+  digitalWrite(SERVO_PIN, HIGH);
+
+  servoTime = micros();
+}
+/******************/
+
+/* FUNCTION COMMAND */
+
+void command_setup() {
+    cmdID = Scheduler.startLoop(command_loop, sizeof cmdStack, cmdStack, cmdContext);
+}
+
+void command_loop() {
+  while ( Serial.available() < 1 ) yield();
+
+  char ctype = Serial.read();
+
+  switch ( ctype ){
+      case 'a':
+        servo_attach();
+      return;
+
+      case 'd':
+        servo_detach();
+      return;
+
+      case '0' ... '9': break;
+      
+      default: return;
+  }
+
+  yield();
+  char cval[5];
+  byte i = 1;
+  cval[0] = ctype;
+  while( i < 3 ) {
+      while ( Serial.available() < 1) yield();
+      cval[i] = Serial.read();
+      if ( cval[i] == '\n' ) break;
+      ++i;
+      yield();
+  }
+  
+  cval[i] = '\0';
+
+  unsigned long value = atoi(cval);
+  yield();
+  if ( value > 180 ) value = 180;
+  servoTon = SERVO_MIN + (((((SERVO_MAX- SERVO_MIN) * 10) / 18) * value)>>2)/25;;
+  yield();
+}
+
+/********************/
+
+void setup() {
+  Serial.begin(9600);
+  pinMode(13, OUTPUT);
+  command_setup();
+  servo_setup();
+}
+
+//alive
+void loop() {
+  digitalWrite(13, HIGH);
+  delay(350);
+  digitalWrite(13, LOW);
+  delay(350);
+}
+
+
+
diff --git a/examples/SchedulerStaticMemory/SchedulerStaticMemory.ino b/examples/SchedulerStaticMemory/SchedulerStaticMemory.ino
new file mode 100644
index 0000000..b01762a
--- /dev/null
+++ b/examples/SchedulerStaticMemory/SchedulerStaticMemory.ino
@@ -0,0 +1,45 @@
+/*
+ Scheduler Static Memory
+
+ Demonstrates to create task in static memory
+
+ Hardware required :
+ Serial
+
+ created 2 fed 2016
+*/
+
+// Include Scheduler since we want to manage multiple tasks.
+#include <Scheduler.h>
+
+// loop2 safe way, use Separate stack memory and context
+TaskContext taskContext2;
+TaskStack taskStack2[ DEFAULT_STACK_SIZE ];
+
+// loop3 unsafe, use stack and context in same memory
+TaskStack taskStack3[ DEFAULT_STACK_SIZE + TASK_CONTEXT_SIZE];
+
+void setup() {
+  Serial.begin(9600);
+
+  //only .startLoop() can lanch task in static memory
+  //.start() lanch task always in dynamic memory
+  Scheduler.startLoop(loop2, DEFAULT_STACK_SIZE, taskStack2, taskContext2);
+  Scheduler.startLoop(loop3, sizeof taskStack3 , taskStack3);  
+
+}
+
+void loop() {
+  Serial.println(F("loop"));
+  delay(1000);
+}
+
+void loop2() {
+  Serial.println(F("loop2"));
+  delay(1000);
+}
+
+void loop3() {
+  Serial.println(F("loop3"));
+  delay(1000);
+}
diff --git a/keywords.txt b/keywords.txt
index 6134de2..997e6ba 100644
--- a/keywords.txt
+++ b/keywords.txt
@@ -2,6 +2,16 @@
 # Syntax Coloring Map For Scheduler
 #######################################
 
+#######################################
+# DataTypes (LITERAL2)
+#######################################
+
+tid_t	LITERAL2		RESERVED_WORD_2
+sem_t	LITERAL2		RESERVED_WORD_2
+mutex_t	LITERAL2		RESERVED_WORD_2
+TaskStack	LITERAL2		RESERVED_WORD_2
+TaskContext	LITERAL2		RESERVED_WORD_2
+
 #######################################
 # Datatypes (KEYWORD1)
 #######################################
@@ -13,8 +23,45 @@ Scheduler	KEYWORD1	Scheduler
 #######################################
 
 startLoop	KEYWORD2
+start	KEYWORD2
+stackPtr	KEYWORD2
+stackEnd	KEYWORD2
+contextPtr	KEYWORD2
+heapStart	KEYWORD2
+heapEnd	KEYWORD2
+taskStop	KEYWORD2
+taskResume	KEYWORD2
+setPriority	KEYWORD2
+taskPriority	KEYWORD2
+switchTask	KEYWORD2
+sem_set	KEYWORD2
+sem_get	KEYWORD2
+sem_op	KEYWORD2
+mutex_init	KEYWORD2
+mutex_lock	KEYWORD2
+mutex_unlock	KEYWORD2
 
 #######################################
 # Constants (LITERAL1)
 #######################################
 
+#######################################
+# Constant (LITERAL2)
+#######################################
+
+DEFAULT_STACK_SIZE	LITERAL2		RESERVED_WORD_2
+TASK_CONTEXT_SIZE	LITERAL2		RESERVED_WORD_2
+
+#######################################
+# Function (KEYWORD3)
+#######################################
+
+loop1	KEYWORD3		RESERVED_WORD
+loop2	KEYWORD3		RESERVED_WORD
+loop3	KEYWORD3		RESERVED_WORD
+loop4	KEYWORD3		RESERVED_WORD
+loop5	KEYWORD3		RESERVED_WORD
+loop6	KEYWORD3		RESERVED_WORD
+loop7	KEYWORD3		RESERVED_WORD
+loop8	KEYWORD3		RESERVED_WORD
+loop9	KEYWORD3		RESERVED_WORD
diff --git a/library.properties b/library.properties
index c9ee1d5..204d27f 100644
--- a/library.properties
+++ b/library.properties
@@ -1,9 +1,9 @@
 name=Scheduler
-version=0.4.4
+version=0.4.5
 author=Arduino
 maintainer=Arduino <info@arduino.cc>
-sentence=Allows multiple tasks to run at the same time, without interrupting each other. For Arduino sam and samd architectures only (Due, Zero...).
+sentence=Allows multiple tasks to run at the same time, without interrupting each other. For Arduino avr/sam and samd architectures only (Uno, Due, Zero...).
 paragraph=The Scheduler library enables the Arduino to run multiple functions at the same time. This allows tasks to happen without interrupting each other.</br>This is a cooperative scheduler in that the CPU switches from one task to another. The library includes methods for passing control between tasks.
 category=Other
 url=http://www.arduino.cc/en/Reference/Scheduler
-architectures=sam,samd
+architectures=avr,sam,samd
diff --git a/src/Scheduler.cpp b/src/Scheduler.cpp
index 6d7d676..b92afe1 100644
--- a/src/Scheduler.cpp
+++ b/src/Scheduler.cpp
@@ -5,7 +5,7 @@
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  *
- *      http://www.apache.org/licenses/LICENSE-2.0
+ *	  http://www.apache.org/licenses/LICENSE-2.0
  *
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
@@ -14,55 +14,408 @@
  * limitations under the License.
  */
 
+
+/* work in progress */
+// Manca:   ContextSwitch per Mega -> da testare -> controllare call convenction per i puntatori a funzione come argomento, adesso 25,24,23
+//      :   Volore di ritorno -> ok
+//      :   Memoria Statica -> ok
+//      :   stackGuard -> testare
+//      :   statoTask -> stop/pending/start -> da testare -> uno deve rimanere attivo
+//      :   ?tick
+//      :   ?Priorità
+//      :   ?Semafori -> chiedere al prof
+//      :   ?watchdog
+//      :   ?preemptive
+/* ***************** */
+
 #include "Scheduler.h"
 
+/* FLAGS
+ * |0|0|0|000|00|
+ *  D O U  P   S
+ *  i v n  r   t
+ *  n e u  i   a
+ *  a r s  o   t
+ *  m f e  r   e
+ *  i l d  i
+ *  c o    t
+ *    w    y
+*/
+
+#define TASK_FLAG_DINAMIC  0x80
+#define TASK_FLAG_STOP     0x10  
+#define TASK_FLAG_PRIORITY 0x07
+
 extern "C" {
 
-#define NUM_REGS 10	// r4-r11, sp, pc
+#define _NONINLINE_ __attribute__((noinline))
+#define _NAKED_	    __attribute__((naked))
+#define _UNUSED_	__attribute__((unused))
+
+#if defined(ARDUINO_ARCH_AVR)
+#ifdef  EIND
+    #define GET_FAR_ADDRESS(var) ({ \
+                                    uint32_t tmp;\
+                                    __asm__ __volatile__( \
+                                                            "ldi    %A0, lo8(%1) \n\t" \
+                                                            "ldi    %B0, hi8(%1) \n\t" \
+                                                            "ldi    %C0, hh8(%1) \n\t" \
+                                                            "clr    %D0          \n\t" \
+                                                            : \
+                                                            "=d" (tmp) \
+                                                            : \
+                                                            "p"  (&(var)) \
+                                                        ); \
+                                    tmp;\
+                                 })
+#endif
+
+#ifdef  EIND
+	#define NUM_REGS 44 // r0/31 + sp(2) + pc(2) + data(2) + ftask(2)
+    #define SPE_REG 40
+    #define PCE_REG 41
+    #define DATAE_REG 42
+	#define TASKFE_REG 43
+    
+    #if CONFIG_AVR_OPTIMIZE_COOPERATIVE == 0
+        #define OFFSET_SKIP 71
+    #else
+        #define OFFSET_SKIP 53
+    #endif
+#else
+    #define NUM_REGS 40 // r0/31 + sp(2) + pc(2) + data(2) + ftask(2)
+    
+    #if CONFIG_AVR_OPTIMIZE_COOPERATIVE == 0
+        #define OFFSET_SKIP 66
+    #else
+        #define OFFSET_SKIP 48
+    #endif
+#endif
+    #define SPL_REG 32
+	#define SPH_REG 33
+    #define PCL_REG 34
+	#define PCH_REG 35
+	#define DATAL_REG 36
+	#define DATAH_REG 37
+	#define TASKFL_REG 38
+	#define TASKFH_REG 39
+	
+	#define STACK_EM0 0xFFFF
+	#define STACK_EM1 0xFFFE
+    
+#elif defined(ARDUINO_ARCH_SAM) || defined(ARDUINO_ARCH_SAMD)
+	#define NUM_REGS 10	// r4-r11, sp, pc
+	#define SP_REG 8
+	#define PC_REG 9
+	#define TASKF_REG 0
+	#define DATA_REG 1
+
+	#define STACK_EM0 0xFFFFFFFF
+	#define STACK_EM1 0xFFFFFFFE
+#endif
 
 typedef struct CoopTask {
-	uint32_t regs[NUM_REGS];
+	reg_t regs[NUM_REGS];
 	void* stackPtr;
+    void* stackEnd;
+    reg_t flags;
 	struct CoopTask* next;
 	struct CoopTask* prev;
 } CoopTask;
 
-static CoopTask *cur = 0;
+static CoopTask *ktask = NULL;
+static CoopTask *cur = NULL;
+static CoopTask *taskStopped = NULL;
+#if DEFAULT_LOOP_CONTEXT_STATIC == 1
+static CoopTask __contextloop;
+#endif
 
-static CoopTask* __attribute__((noinline)) coopSchedule(char taskDied) {
+#define HANDLER_DISABLE    0
+#define HANDLER_MALLOC     1
+
+static uint8_t handler = 0;
+static void*   hrarg  = 0;
+static void*   hrret  = 0;
+static tid_t   caller  = 0;
+static uint8_t defpriority = 0;
+
+static CoopTask* _NONINLINE_ coopSchedule(char taskDied) {
 	CoopTask* next = cur->next;
 
-	if (taskDied) {
+	if ( 1 == taskDied && (cur->flags & TASK_FLAG_DINAMIC) ) {
 		// Halt if last task died.
 		if (next == cur)
-			while (1)
-				;
-
+			while(1);
 		// Delete task
-		if (cur->stackPtr)
+        if (cur->stackPtr)
 			free(cur->stackPtr);
 		cur->next->prev = cur->prev;
 		cur->prev->next = cur->next;
 		free(cur);
 	}
-	cur = next;
-	return next;
+    
+    cur = next;
+	return cur;
+}
+
+#if defined(ARDUINO_ARCH_AVR)
+
+static void _NAKED_ _NONINLINE_ coopTaskStart(void) {
+	asm (
+		"movw	r4, r24		;prepare to call ftask(data) \n\t"
+		"movw	30, r4		\n\t"
+		"ldd	r26, Z+34	;increment PC, next call ret without call ftask(data) \n\t"
+		"ldd	r27, Z+35	\n\t"
+#if CONFIG_AVR_OPTIMIZE_COOPERATIVE == 0
+		"adiw	r26, 60		;offset ret\n\t"
+#else
+        "adiw	r26, 42		;offset ret\n\t"
+#endif
+#ifdef EIND
+        "adiw	r26, 11		;offset ret\n\t"
+#else
+        "adiw	r26, 6		;offset ret\n\t"
+#endif
+		"movw	r18, r26	\n\t"
+		"std	Z+34, r18	\n\t"
+		"std	Z+35, r19	\n\t"
+#ifdef EIND
+        "ldd	r23, Z+36	;data \n\t"
+		"ldd	r24, Z+37	\n\t"
+        "ldd	r25, Z+42	\n\t"
+#else
+		"ldd	r24, Z+36	;data \n\t"
+		"ldd	r25, Z+37	\n\t"
+#endif
+		
+#ifdef EIND
+        "movw	r6, Z+43	\n\t"
+        "out    EIND, r6    \n\t"
+#endif
+        "ldd	r6, Z+38	\n\t"
+		"ldd	r7, Z+39	\n\t"
+        "movw	r30, r6		\n\t"
+#ifdef EIND
+        "eicall			;ftask(data) \n\t"
+#else
+		"icall			;ftask(data) \n\t"
+#endif
+		"ldi	r24, 1		;current task die\n\t"
+		"call   coopSchedule	;and get next coop\n\t"
+		"movw	r4, r24		;r25:r24 cur task\n\t"
+		"movw	30, r4		;load context \n\t"
+		"ldd	r6, Z+32	;load stack\n\t"
+        "in     r0, __SREG__    ;safe interrupt\n\t"
+        "cli                \n\t"
+		"mov	r28,r6		\n\t"
+		"out	__SP_L__, r6	\n\t"
+		"ldd	r6, Z+33	\n\t"
+		"mov	r29,r6		\n\t"
+		"out	__SP_H__, r6	\n\t"
+#ifdef EIND
+        "ldd    r6, Z+40    ;load 3byte stack\n\t"
+        "out	EIND, r6	\n\t"
+#endif
+        "out    __SREG__, r0    \n\t"
+#if CONFIG_AVR_OPTIMIZE_COOPERATIVE == 0
+		"ldd	r0, Z+0		;load register \n\t"
+		"ldd	r1, Z+1		\n\t"
+#else
+        "clr    r1          \n\t"
+#endif
+		"ldd	r2, Z+2		\n\t"
+		"ldd	r3, Z+3		\n\t"
+		"ldd	r4, Z+4		\n\t"
+		"ldd	r5, Z+5		\n\t"
+		"ldd	r6, Z+6		\n\t"
+		"ldd	r7, Z+7		\n\t"
+		"ldd	r8, Z+8		\n\t"
+		"ldd	r9, Z+9		\n\t"
+		"ldd	r10, Z+10	\n\t"
+		"ldd	r11, Z+11	\n\t"
+		"ldd	r12, Z+12	\n\t"
+		"ldd	r13, Z+13	\n\t"
+		"ldd	r14, Z+14	\n\t"
+		"ldd	r15, Z+15	\n\t"
+		"ldd	r16, Z+16	\n\t"
+		"ldd	r17, Z+17	\n\t"
+#if CONFIG_AVR_OPTIMIZE_COOPERATIVE == 0
+		"ldd	r18, Z+18	\n\t"
+		"ldd	r19, Z+19	\n\t"
+		"ldd	r20, Z+20	\n\t"
+		"ldd	r21, Z+21	\n\t"
+		"ldd	r22, Z+22	\n\t"
+		"ldd	r23, Z+23	\n\t"
+		"ldd	r24, Z+24	\n\t"
+		"ldd	r25, Z+25	\n\t"
+		"ldd	r26, Z+26	\n\t"
+		"ldd	r27, Z+27	\n\t"
+#endif
+		"ldd	r28, Z+28	\n\t"
+		"ldd	r29, Z+29	\n\t"
+#if CONFIG_AVR_OPTIMIZE_COOPERATIVE == 0
+		"push   r4		\n\t"
+		"push   r5		\n\t"
+		"ldd	r4, Z+30	\n\t"
+		"ldd	r5, Z+31	\n\t"
+		"movw   r30, r4		\n\t"
+		"pop	r5		\n\t"
+		"pop	r4		\n\t"
+#endif
+		"ret			\n\t"
+	);
 }
 
-static void __attribute__((naked)) __attribute__((noinline)) coopTaskStart(void) {
+static void _NAKED_ _NONINLINE_ coopDoYield(CoopTask* curTask _UNUSED_) {
+	asm (
+#if CONFIG_AVR_OPTIMIZE_COOPERATIVE == 0
+		"push   r30		;store context, r25:r24 holds address of next task context \n\t"
+		"push   r31		\n\t"
+#endif
+		"movw   r30, r24	\n\t"
+#if CONFIG_AVR_OPTIMIZE_COOPERATIVE == 0
+		"std	Z+0, r0		\n\t"
+		"std	Z+1, r1		\n\t"
+#endif
+		"std	Z+2, r2		\n\t"
+		"std	Z+3, r3		\n\t"
+		"std	Z+4, r4		\n\t"
+		"std	Z+5, r5		\n\t"
+		"std	Z+6, r6		\n\t"
+		"std	Z+7, r7		\n\t"
+		"std	Z+8, r8		\n\t"
+		"std	Z+9, r9		\n\t"
+		"std	Z+10, r10	\n\t"
+		"std	Z+11, r11	\n\t"
+		"std	Z+12, r12	\n\t"
+		"std	Z+13, r13	\n\t"
+		"std	Z+14, r14	\n\t"
+		"std	Z+15, r15	\n\t"
+		"std	Z+16, r16	\n\t"
+		"std	Z+17, r17	\n\t"
+#if CONFIG_AVR_OPTIMIZE_COOPERATIVE == 0
+		"std	Z+18, r18	\n\t"
+		"std	Z+19, r19	\n\t"
+		"std	Z+20, r20	\n\t"
+		"std	Z+21, r21	\n\t"
+		"std	Z+22, r22	\n\t"
+		"std	Z+23, r23	\n\t"
+		"std	Z+24, r24	\n\t"
+		"std	Z+25, r25	\n\t"
+		"std	Z+26, r26	\n\t"
+		"std	Z+27, r27	\n\t"
+#endif
+		"std	Z+28, r28	\n\t"
+		"std	Z+29, r29	\n\t"
+#if CONFIG_AVR_OPTIMIZE_COOPERATIVE == 0
+		"pop	r5		\n\t"
+		"pop	r4		\n\t"
+		"std	Z+30, r4	\n\t"
+		"std	Z+31, r5	\n\t"
+#endif
+		"in	 r4, __SP_L__	;store stack \n\t"
+		"std	Z+32, r4	\n\t"
+		"in	 r4, __SP_H__	\n\t"
+		"std	Z+33, r4	\n\t"
+#ifdef EIND
+        "in	 r4, EIND	\n\t"
+		"std	Z+40, r4	\n\t"
+#endif
+		"ldi	r24, 0		;next coop \n\t"
+		"call  coopSchedule	\n\t"
+        "in     r0, __SREG__    ;safe interrupt\n\t"
+        "cli                \n\t"
+		"movw	r4, r24		;load context \n\t"
+		"movw	30, r4		\n\t"
+		"ldd	r6, Z+32	\n\t"
+		"mov	r28,r6		\n\t"
+		"out	__SP_L__, r6	;load stack\n\t"
+		"ldd	r6, Z+33	\n\t"
+		"mov	r29,r6		\n\t"
+		"out	__SP_H__, r6	\n\t"
+#ifdef EIND
+        "ldd	r6,Z+40	\n\t"
+        "out	EIND,r6	\n\t"
+#endif
+		"ldd	r6, Z+34	;load pc\n\t"
+		"ldd	r7, Z+35	\n\t"
+#ifdef EIND
+        "ldd	r6,Z+41	\n\t"
+        "out	EIND,r6	\n\t"
+#endif
+		"movw	r30, r6		\n\t"
+        "out    __SREG__, r0    \n\t"
+#ifdef EIND
+        "eicall			;call coopTaskStart if begin else return after icall \n\t"
+#else
+		"icall			;call coopTaskStart if begin else return after icall \n\t"
+#endif
+		"movw   r30, r4		;need reload structure \n\t"
+#if CONFIG_AVR_OPTIMIZE_COOPERATIVE == 0
+		"ldd	r0, Z+0		;load register \n\t"
+		"ldd	r1, Z+1		\n\t"
+#else
+        "clr    r1          \n\t"
+#endif
+		"ldd	r2, Z+2		\n\t"
+		"ldd	r3, Z+3		\n\t"
+		"ldd	r4, Z+4		\n\t"
+		"ldd	r5, Z+5		\n\t"
+		"ldd	r6, Z+6		\n\t"
+		"ldd	r7, Z+7		\n\t"
+		"ldd	r8, Z+8		\n\t"
+		"ldd	r9, Z+9		\n\t"
+		"ldd	r10, Z+10	\n\t"
+		"ldd	r11, Z+11	\n\t"
+		"ldd	r12, Z+12	\n\t"
+		"ldd	r13, Z+13	\n\t"
+		"ldd	r14, Z+14	\n\t"
+		"ldd	r15, Z+15	\n\t"
+		"ldd	r16, Z+16	\n\t"
+		"ldd	r17, Z+17	\n\t"
+#if CONFIG_AVR_OPTIMIZE_COOPERATIVE == 0
+		"ldd	r18, Z+18	\n\t"
+		"ldd	r19, Z+19	\n\t"
+		"ldd	r20, Z+20	;load register \n\t"
+		"ldd	r21, Z+21	\n\t"
+		"ldd	r22, Z+22	\n\t"
+		"ldd	r23, Z+23	\n\t"
+		"ldd	r24, Z+24	\n\t"
+		"ldd	r25, Z+25	\n\t"
+		"ldd	r26, Z+26	\n\t"
+		"ldd	r27, Z+27	\n\t"
+#endif
+		"ldd	r28, Z+28	\n\t"
+		"ldd	r29, Z+29	\n\t"
+#if CONFIG_AVR_OPTIMIZE_COOPERATIVE == 0
+		"push   r4		\n\t"
+		"push   r5		\n\t"
+		"ldd	r4, Z+30	\n\t"
+		"ldd	r5, Z+31	\n\t"
+		"movw   r30, r4		\n\t"
+		"pop	r5		\n\t"
+		"pop	r4		\n\t"
+#endif
+		"ret			\n\t"
+		);
+}
+
+#elif defined(ARDUINO_ARCH_SAM) || defined(ARDUINO_ARCH_SAMD)
+
+static void _NAKED_ _NONINLINE_ coopTaskStart(void) {
 	asm (
 		"mov   r0, r5;"
 		"blx   r4;"
 		/* schedule. */
-		"mov   r0, #1;"      /* returned from task func: task done */
-		"bl    coopSchedule;"
+		"mov   r0, #1;"	  /* returned from task func: task done */
+		"bl	coopSchedule;"
 		/* r0 holds address of next task context */
 #if defined(ARDUINO_ARCH_SAMD)
 		/* for cortex m0, ldm and stm are restricted to low registers */
 		/* load high registers */
-		"add   r0, #16;"     /* they are 4 words higher in memory */
+		"add   r0, #16;"	 /* they are 4 words higher in memory */
 		"ldmia r0, {r1-r6};" /* load them in low registers first... */
-		"mov   r8, r1;"      /* ...and move them into high registers... */
+		"mov   r8, r1;"	  /* ...and move them into high registers... */
 		"mov   r9, r2;"
 		"mov   r10, r3;"
 		"mov   r11, r4;"
@@ -77,18 +430,18 @@ static void __attribute__((naked)) __attribute__((noinline)) coopTaskStart(void)
 		/* restore task stack */
 		"mov   sp, r12;"
 		/* resume task */
-		"bx    lr;"
+		"bx	lr;"
 	);
 }
 
-static void __attribute__((naked)) __attribute__((noinline)) coopDoYield(CoopTask* curTask) {
+static void _NAKED_ _NONINLINE_ coopDoYield(CoopTask* curTask) {
 	asm (
 		"mov   r12, sp;"
 #if defined(ARDUINO_ARCH_SAMD)
 		/* store low registers */
 		"stmia r0, {r4-r7};"
 		/* store high registers */
-		"mov   r1, r8;"      /* move them to low registers first. */
+		"mov   r1, r8;"	  /* move them to low registers first. */
 		"mov   r2, r9;"
 		"mov   r3, r10;"
 		"mov   r4, r11;"
@@ -99,15 +452,15 @@ static void __attribute__((naked)) __attribute__((noinline)) coopDoYield(CoopTas
 		"stmia r0, {r4-r12, lr};"
 #endif
 		/* schedule. */
-		"mov   r0, #0;"      /* previous task did not complete */
-		"bl    coopSchedule;"
+		"mov   r0, #0;"	  /* previous task did not complete */
+		"bl	coopSchedule;"
 		/* r0 holds address of next task context */
 #if defined(ARDUINO_ARCH_SAMD)
 		/* for cortex m0, ldm and stm are restricted to low registers */
 		/* load high registers */
-		"add   r0, #16;"     /* they are 4 words higher in memory */
+		"add   r0, #16;"	 /* they are 4 words higher in memory */
 		"ldmia r0, {r1-r6};" /* load them in low registers first... */
-		"mov   r8, r1;"      /* ...and move them into high registers... */
+		"mov   r8, r1;"	  /* ...and move them into high registers... */
 		"mov   r9, r2;"
 		"mov   r10, r3;"
 		"mov   r11, r4;"
@@ -122,57 +475,210 @@ static void __attribute__((naked)) __attribute__((noinline)) coopDoYield(CoopTas
 		/* restore task stack */
 		"mov   sp, r12;"
 		/* resume task */
-		"bx    lr;"
+		"bx	lr;"
 	);
 }
+#endif //ARCH
 
 static int coopInit(void) {
 	CoopTask* task;
-
-	task = reinterpret_cast<CoopTask *>(malloc(sizeof(CoopTask)));
+    
+#if DEFAULT_LOOP_CONTEXT_STATIC == 1
+	task = &__contextloop;
+    task->flags = 7;
+#else
+    task = reinterpret_cast<CoopTask *>(malloc(sizeof(CoopTask)));
+    
 	if (!task)
 		return 0;
+    task->flags = TASK_FLAG_DINAMIC | 7;
+    
+#endif
 	task->next = task;
 	task->prev = task;
-	task->stackPtr = 0;
-	cur = task;
-
+	task->stackPtr = NULL;
+    task->stackEnd = NULL;
+#ifdef ARDUINO_ARCH_AVR
+	task->regs[SPL_REG] = 0;
+	task->regs[SPH_REG] = 0;
+#ifdef  EIND
+    uint32_t pf = GET_FAR_ADDRESS(coopTaskStart);
+    task->regs[PCL_REG] = (pf + OFFSET_SKIP)        & 0xFF;
+	task->regs[PCH_REG] = (pf + OFFSET_SKIP) >> 8)  & 0xFF;
+    task->regs[PCE_REG] = (pf + OFFSET_SKIP) >> 16) & 0xFF;
+    task->regs[SPE_REG] = 0;
+    task->regs[DATAE_REG] = 0;
+    task->regs[TASKFE_REG] = 0;
+#else
+	task->regs[PCL_REG] = ((uint16_t)(coopTaskStart) + OFFSET_SKIP) & 0xFF;
+	task->regs[PCH_REG] = (((uint16_t)(coopTaskStart) + OFFSET_SKIP) >> 8) & 0xFF;
+#endif
+	task->regs[DATAL_REG] = 0;
+	task->regs[DATAH_REG] = 0;
+	task->regs[TASKFL_REG] = 0;
+	task->regs[TASKFH_REG] = 0;
+#endif
+    
+	ktask = cur = task;
+    
 	return 1;
 }
 
-static int coopSpawn(SchedulerParametricTask taskF, void* taskData, uint32_t stackSz) {
-	uint8_t *stack = (uint8_t*)malloc(stackSz);
-	if (!stack)
-		return 0;
+static void coopOrder(CoopTask* task) {
+    CoopTask* taskHigh = cur;
+    CoopTask* ftask = cur->next;
+    
+    //find high task
+    for (; ftask != cur; ftask = ftask->next)
+        if ( (taskHigh->flags & TASK_FLAG_PRIORITY) < (ftask->flags & TASK_FLAG_PRIORITY) )
+            taskHigh = ftask;
+    
+    //find priority
+    if ( (task->flags & TASK_FLAG_PRIORITY) > (taskHigh->flags & TASK_FLAG_PRIORITY) ) {
+        ftask = taskHigh;
+    }
+    else {
+        for (ftask = taskHigh->next; (ftask != taskHigh) && ((task->flags & TASK_FLAG_PRIORITY) < (ftask->flags & TASK_FLAG_PRIORITY)); ftask = ftask->next);
+    }
+    
+    //push
+    task->next = ftask;
+    task->prev = ftask->prev;
+    ftask->prev->next = task;
+    ftask->prev = task;  
+}
+
+static tid_t coopSpawn(SchedulerParametricTask taskF, void* taskData, uint32_t stackSz, void* stack, void* context) {
+	
+    CoopTask *task = NULL;
+    
+    if ( NULL == stack || NULL == context )
+    {
+        stack = (uint8_t*)malloc(stackSz);
+
+        if (!stack)
+            return 0;
+            
+        task = reinterpret_cast<CoopTask *>(malloc(sizeof(CoopTask)));
+        if (!task) {
+            free(stack);
+            return 0;
+        }
+        
+        task->flags = TASK_FLAG_DINAMIC | defpriority;
+    }
+    else
+    {
+        task = (CoopTask*)(context);
+        task->flags = defpriority;
+    }
 
-	CoopTask *task = reinterpret_cast<CoopTask *>(malloc(sizeof(CoopTask)));
-	if (!task) {
-		free(stack);
-		return 0;
-	}
 	task->stackPtr = stack;
-	task->regs[0] = (uint32_t) taskF;
-	task->regs[1] = (uint32_t) taskData;
-	task->regs[8] = ((uint32_t)(stack + stackSz)) & ~7;
-	task->regs[9] = (uint32_t) & coopTaskStart;
 
-	task->prev = cur;
-	task->next = cur->next;
-	cur->next->prev = task;
-	cur->next = task;
+#ifdef ARDUINO_ARCH_AVR
+
+    
+#ifdef  EIND
+    uint32_t pf = GET_FAR_ADDRESS(coopTaskStart);
+    task->regs[PCL_REG] = pf & 0xFF;
+	task->regs[PCH_REG] = (pf >> 8) & 0xFF;
+    task->regs[PCE_REG] = (pf >> 16) & 0xFF;
+    pf = GET_FAR_ADDRESS(taskF);
+    task->regs[TASKFL_REG] = pf & 0xFF;
+	task->regs[TASKFH_REG] = (pf >> 8) & 0xFF;
+    task->regs[TASKFE_REG] = (pf >> 16) & 0xFF;
+    pf = GET_FAR_ADDRESS(taskData);
+	task->regs[DATAL_REG]  = pf & 0xFF;
+	task->regs[DATAH_REG]  = (pf >> 8) & 0xFF;
+    task->regs[DATAE_REG]  = (pf >> 16) & 0xFF;
+#else
+    task->regs[TASKFL_REG] = (uint16_t)(taskF) & 0xFF;
+	task->regs[TASKFH_REG] = ((uint16_t)(taskF) >> 8) & 0xFF;
+	task->regs[DATAL_REG]  = (uint16_t)(taskData) & 0xFF;
+	task->regs[DATAH_REG]  = ((uint16_t)(taskData) >> 8) & 0xFF;
+	task->regs[PCL_REG]	= (uint16_t)(coopTaskStart) & 0xFF;
+	task->regs[PCH_REG]	= ((uint16_t)(coopTaskStart) >> 8) & 0xFF;
+	task->regs[SPL_REG]	= (uint16_t)((uint8_t*)(stack) + stackSz - 1) & 0xFF;
+	task->regs[SPH_REG]	= ((uint16_t)((uint8_t*)(stack) + stackSz - 1 ) >> 8) & 0xFF;
+#endif	
+
+#elif defined(ARDUINO_ARCH_SAM) || defined(ARDUINO_ARCH_SAMD)
+	task->regs[TASKF_REG] = (uint32_t) taskF;
+	task->regs[DATA_REG]  = (uint32_t) taskData;
+	task->regs[SP_REG]	= ((uint32_t)((uint32_t*)(stack) + stackSz)) & ~7;
+	task->regs[PC_REG]	= (uint32_t) & coopTaskStart;
+
+#endif
+    
+    task->stackEnd = stack;
+    
+    //add in priority 
+    coopOrder(task);
 
 	// These are here so compiler is sure that function is
 	// referenced in both variants (cancels a warning)
-	if (stackSz == 0xFFFFFFFF)
+	if (stackSz == STACK_EM0)
 		coopSchedule(0);
-	if (stackSz == 0xFFFFFFFE)
+	if (stackSz == STACK_EM1)
 		coopSchedule(1);
 
-	return 1;
+	return (tid_t)(task);
+}
+
+void *__kmalloc(size_t len)
+{
+    #if defined(ARDUINO_ARCH_AVR)
+    if ( cur == ktask )
+        #pragma pop_macro("malloc")
+        return malloc(len);
+        #pragma push_macro("malloc")
+        #undef malloc
+        #define malloc __kmalloc
+    #else
+        return malloc(len);
+    #endif
+    handler = HANDLER_MALLOC;
+    hrarg  = &len;
+    caller = (tid_t)(cur);
+    switchTask((tid_t)(ktask));
+    return hrret;
+}
+
+static void ksys(void)
+{
+    if ( handler == HANDLER_MALLOC ){
+        size_t* l = (size_t*)(hrarg);
+        #if defined(ARDUINO_ARCH_AVR)
+            #pragma pop_macro("malloc")
+            hrret = malloc(*l);
+            #pragma push_macro("malloc")
+            #undef malloc
+            #define malloc __kmalloc
+        #else
+            hrret = malloc(*l);
+        #endif
+    }
+    handler = HANDLER_DISABLE;
+    switchTask(caller);
+}
+
+void switchTask(tid_t taskid) {
+    CoopTask* task = ( 0 != taskid ) ? (CoopTask*)(taskid) : cur;
+    CoopTask* excur = cur;
+    
+    if ( task->flags & TASK_FLAG_STOP ) return;
+    
+    for(; cur->next != task; cur = cur->next);
+    
+    coopDoYield(excur);
+    while ( handler != HANDLER_DISABLE && cur == ktask)
+        ksys();
 }
 
 void yield(void) {
 	coopDoYield(cur);
+    while ( handler != HANDLER_DISABLE && cur == ktask )
+        ksys();
 }
 
 }; // extern "C"
@@ -182,27 +688,212 @@ SchedulerClass::SchedulerClass() {
 }
 
 static void startLoopHelper(void *taskData) {
+
 	SchedulerTask task = reinterpret_cast<SchedulerTask>(taskData);
+
 	while (true)
 		task();
 }
 
-void SchedulerClass::startLoop(SchedulerTask task, uint32_t stackSize) {
-	coopSpawn(startLoopHelper, reinterpret_cast<void *>(task), stackSize);
+tid_t SchedulerClass::startLoop(SchedulerTask task, stacksz_t stackSize)
+{
+    if ( stackSize < DEFAULT_MIN_STACK_SIZE)
+         stackSize = DEFAULT_MIN_STACK_SIZE;
+	return coopSpawn(startLoopHelper, reinterpret_cast<void *>(task), stackSize, NULL, NULL);
+}
+
+tid_t SchedulerClass::startLoop(SchedulerTask task, stacksz_t stackSize, TaskStack* stack)
+{
+    if ( NULL == stack ) return 0;
+    if ( stackSize < sizeof(CoopTask) + DEFAULT_MIN_STACK_SIZE) return 0;
+	return coopSpawn(startLoopHelper, reinterpret_cast<void *>(task), stackSize, (reg_t*)(stack) + sizeof(CoopTask), stack);
+}
+
+tid_t SchedulerClass::startLoop(SchedulerTask task, stacksz_t stackSize, TaskStack* stack, uint8_t* context)
+{
+    if ( NULL == stack ) return 0;
+    if ( stackSize < DEFAULT_MIN_STACK_SIZE) return 0;
+	return coopSpawn(startLoopHelper, reinterpret_cast<void *>(task), stackSize, stack, context);
 }
 
 static void startTaskHelper(void *taskData) {
 	SchedulerTask task = reinterpret_cast<SchedulerTask>(taskData);
 	task();
+#ifdef ARDUINO_ARCH_AVR   
+    yield();
+#endif
+}
+
+tid_t SchedulerClass::start(SchedulerTask task, stacksz_t stackSize) {
+    if ( stackSize < DEFAULT_MIN_STACK_SIZE)
+         stackSize = DEFAULT_MIN_STACK_SIZE;
+	return coopSpawn(startTaskHelper, reinterpret_cast<void *>(task), stackSize, NULL, NULL);
+}
+
+tid_t SchedulerClass::start(SchedulerParametricTask task, void *taskData, stacksz_t stackSize) {
+    if ( stackSize < DEFAULT_MIN_STACK_SIZE)
+         stackSize = DEFAULT_MIN_STACK_SIZE;
+    return coopSpawn(task, taskData, stackSize, NULL, NULL);
 }
 
-void SchedulerClass::start(SchedulerTask task, uint32_t stackSize) {
-	coopSpawn(startTaskHelper, reinterpret_cast<void *>(task), stackSize);
+uint32_t SchedulerClass::stackPtr(void) {
+    uint32_t sp = (uint32_t)(cur->regs[SPH_REG]) << 8;
+    sp |= cur->regs[SPL_REG];
+    return sp;
 }
 
-void SchedulerClass::start(SchedulerParametricTask task, void *taskData, uint32_t stackSize) {
-	coopSpawn(task, taskData, stackSize);
+uint32_t SchedulerClass::stackEnd(void) {
+    return (cur->stackEnd == NULL) ? heapEnd() + 1 : (uint32_t)(cur->stackEnd);
+}
+
+uint32_t SchedulerClass::contextPtr(void) {
+    return (uint32_t)(cur);
+}
+
+uint32_t SchedulerClass::heapStart(void) {
+    extern char* __malloc_heap_start;
+    return (uint32_t)(__malloc_heap_start);
+}
+
+uint32_t SchedulerClass::heapEnd(void) {
+    extern char* __brkval;
+    return (uint32_t)(__brkval);
+}
+
+void SchedulerClass::taskStop(tid_t taskid) {
+    CoopTask* task = ( 0 != taskid ) ? (CoopTask*)(taskid) : cur;
+    CoopTask* prev = cur->prev;
+    
+    task->flags |= TASK_FLAG_STOP;
+    if ( task == task->next ) 
+        while(1);
+    
+    //pull
+    task->next->prev = task->prev;
+    task->prev->next = task->next;
+    
+    //push to stop
+    if ( taskStopped == NULL ) {
+        taskStopped = task;
+        task->next = task;
+        task->prev = task;
+    }
+    else {
+        task->prev = taskStopped;
+        task->next = taskStopped->next;
+        taskStopped->next->prev = task;
+        taskStopped->next = task;    
+    }
+    
+    if ( task == cur ){
+        cur = prev;
+        coopDoYield(task);
+    }
+    return;
+}
+
+void SchedulerClass::taskResume(tid_t taskid) {
+    CoopTask* task = (CoopTask*)(taskid);
+    
+    if ( !(task->flags & TASK_FLAG_STOP) ) return;
+    task->flags &= ~TASK_FLAG_STOP;
+    
+    //pull
+    if ( task == taskStopped ) {
+        if ( task == task->next )
+            taskStopped = NULL;
+        else
+            taskStopped = taskStopped->next;
+    }
+    task->next->prev = task->prev;
+    task->prev->next = task->next;
+    
+    //push to run
+    coopOrder(task);
+    
+    return;
+}
+
+void SchedulerClass::setPriority(uint8_t priority) {
+    defpriority = priority & TASK_FLAG_PRIORITY;
+    return;
+}
+
+void SchedulerClass::taskPriority(tid_t taskid, uint8_t priority) {
+    CoopTask* task = ( 0 != taskid ) ? (CoopTask*)(taskid) : cur;
+    
+    priority &= TASK_FLAG_PRIORITY;
+    task->flags &= ~TASK_FLAG_PRIORITY;
+    task->flags |= priority;
+    if ( task == task->next ) return;
+    if ( cur == task ) {
+         cur = cur->next;
+         taskid = 0;
+    }
+    //pull
+    task->next->prev = task->prev;
+    task->prev->next = task->next;
+    //push
+    coopOrder(task);
+    if ( 0 == taskid ) cur = task;
+}
+
+void SchedulerClass::wait(uint32_t ms) {
+	uint32_t start = micros();
+
+	while (ms > 0) {
+		while ( ms > 0 && (micros() - start) >= 1000) {
+			ms--;
+			start += 1000;
+		}
+	}
 }
 
 SchedulerClass Scheduler;
 
+#undef TASK_FLAG_DINAMIC
+#undef TASK_FLAG_STOP
+#undef TASK_FLAG_PRIORITY
+
+
+#undef NUMREGS
+#undef STACK_EM0
+#undef STACK_EM1
+
+#ifdef ARDUINO_ARCH_AVR 
+#ifdef EIND
+    #undef SPE_REG
+    #undef PCE_REG
+    #undef DATAE_REG
+	#undef TASKFE_REG
+    #undef GET_FAR_ADDRESS
+#endif
+    #undef NUM_REGS
+    #undef SPL_REG
+	#undef SPH_REG
+    #undef PCL_REG
+	#undef PCH_REG
+	#undef DATAL_REG
+	#undef DATAH_REG
+	#undef TASKFL_REG
+	#undef TASKFH_REG
+#elif defined(ARDUINO_ARCH_SAM) || defined(ARDUINO_ARCH_SAMD)
+	#undef NUM_REGS
+	#undef SP_REG
+	#undef PC_REG
+	#undef TASKF_REG
+	#undef DATA_REG
+#endif
+
+#undef HANDLER_DISABLE
+#undef HANDLER_MALLOC
+
+#ifdef _NAKED_
+#undef _NAKED_
+#endif
+#ifdef _NONINLINE_
+#undef _NONINLINE_
+#endif
+#ifdef _UNUSED_
+#undef _UNUSED_
+#endif
diff --git a/src/Scheduler.h b/src/Scheduler.h
index 9a45cbb..35b1cbc 100644
--- a/src/Scheduler.h
+++ b/src/Scheduler.h
@@ -5,7 +5,7 @@
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  *
- *      http://www.apache.org/licenses/LICENSE-2.0
+ *	  http://www.apache.org/licenses/LICENSE-2.0
  *
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
@@ -19,22 +19,108 @@
 
 #include <Arduino.h>
 
+#define CONFIG_AVR_OPTIMIZE_COOPERATIVE 0
+
+#define DEFAULT_MIN_STACK_SIZE 50
+#ifndef DEFAULT_LOOP_CONTEXT_STATIC
+#define DEFAULT_LOOP_CONTEXT_STATIC 1
+#endif
+
+#if defined(ARDUINO_ARCH_AVR)
+
+    #ifdef malloc
+        #pragma push_macro("malloc")
+        #undef malloc
+    #else
+        #define malloc malloc
+        #pragma push_macro("malloc")
+        #undef malloc
+    #endif
+    
+    #define malloc __kmalloc
+    
+    typedef uint8_t reg_t;
+	typedef uint16_t stacksz_t;
+    
+	#if ((RAMEND - RAMSTART) < 1000)
+		#pragma GCC error "board is not supported"
+		//invoche a real fatal error
+		#include "board is not supported"	
+	#elif ((RAMEND - RAMSTART) < 2000)
+		#define DEFAULT_STACK_SIZE 200
+	#elif ((RAMEND - RAMSTART) < 3000)
+		#define DEFAULT_STACK_SIZE 250
+	#elif ((RAMEND - RAMSTART) < 10000)
+		#define DEFAULT_STACK_SIZE 900
+	#else
+		#define DEFAULT_STACK_SIZE 1024
+	#endif
+    
+    #ifdef EIND
+        #define TASK_CONTEXT_SIZE (44 + 13)
+        typedef uint32_t tid_t;
+    #else
+        #define TASK_CONTEXT_SIZE (40 + 9)
+        typedef uint16_t tid_t;
+    #endif
+    
+#elif defined(ARDUINO_ARCH_SAM) || defined(ARDUINO_ARCH_SAMD)
+	#define DEFAULT_STACK_SIZE 1024
+    #define TASK_CONTEXT_SIZE (10 + 20)
+    
+    typedef uint32_t reg_t;
+	typedef uint32_t stacksz_t;
+    typedef uint32_t tid_t;
+#endif
+
+
 extern "C" {
 	typedef void (*SchedulerTask)(void);
 	typedef void (*SchedulerParametricTask)(void *);
+    
+    #if defined(ARDUINO_ARCH_AVR)
+        void *__kmalloc(size_t len);
+    #endif
+    
+    typedef reg_t TaskStack;
+    typedef reg_t TaskContext[TASK_CONTEXT_SIZE];
+    void switchTask(tid_t taskid);
+    
+    typedef uint8_t sem_t;
+    typedef sem_t mutex_t;
 }
 
 class SchedulerClass {
 public:
 	SchedulerClass();
-	static void startLoop(SchedulerTask task, uint32_t stackSize = 1024);
-	static void start(SchedulerTask task, uint32_t stackSize = 1024);
-	static void start(SchedulerParametricTask task, void *data, uint32_t stackSize = 1024);
-
+	static tid_t startLoop(SchedulerTask task, stacksz_t stackSize = DEFAULT_STACK_SIZE);
+    static tid_t startLoop(SchedulerTask task, stacksz_t stackSize, TaskStack* stack);
+    static tid_t startLoop(SchedulerTask task, stacksz_t stackSize, TaskStack* stack, uint8_t* context);
+	static tid_t start(SchedulerTask task, stacksz_t stackSize = DEFAULT_STACK_SIZE);
+	static tid_t start(SchedulerParametricTask task, void *data, stacksz_t stackSize = DEFAULT_STACK_SIZE);
+    uint32_t stackPtr(void);
+    uint32_t stackEnd(void);
+    uint32_t contextPtr(void);
+    uint32_t heapStart(void);
+    uint32_t heapEnd(void);
+    void taskStop(tid_t taskid);
+    void taskResume(tid_t taskid);
+    void setPriority(uint8_t priority);
+    void taskPriority(tid_t taskid, uint8_t priority);
+    void wait(uint32_t ms);
+    static void switchTask(tid_t taskid){::switchTask(taskid);};
 	static void yield() { ::yield(); };
 };
 
 extern SchedulerClass Scheduler;
 
-#endif
+#define sem_set(S, V) ({ S = V; })
+#define sem_get(S)    ({S;})
+#define sem_op(S,O)   ({ (S) += O; while( 0 == (S) ) yield(); )}
 
+#define mutex_init(M)   sem_set(M, 1)
+#define mutex_lock(M)   sem_op(M,-1)
+#define mutex_unlock(M) sem_op(M, +1)
+
+
+#endif