/*---------------------------------------------------------------------------*/
static int
run_trickle(struct propple_socket *s)
{
  clock_time_t interval;
  PT_BEGIN(&s->pt);

  while(1) {
    interval = s->interval << s->interval_scaling;
    if(interval > s->interval_max) {
      interval = s->interval_max;
    }
    set_timer(s, &s->interval_timer, interval);
    set_timer(s, &s->t, interval / 2 + (random_rand() % (interval / 2)));

    s->duplicates = 0;
    PT_YIELD(&s->pt); /* Wait until listen timeout */
    if(s->duplicates < s->duptheshold) {
      send(s);
    }
    interval = s->interval << s->interval_scaling;
    if(interval < s->interval_max) {
      s->interval_scaling++;
    }
    PT_YIELD(&s->pt); /* Wait until interval timer expired. */
  }

  PT_END(&s->pt);
}

/*---------------------------------------------------------------------------*/
static int
input_pt(struct http_socket *s,
         const uint8_t *inputptr, int inputdatalen)
{
  int i;
  PT_BEGIN(&s->pt);

  /* Parse the header */
  s->header_received = 0;
  do {
    for(i = 0; i < inputdatalen; i++) {
      if(!PT_SCHEDULE(parse_header_byte(s, inputptr[i]))) {
        s->header_received = 1;
        break;
      }
    }
    inputdatalen -= i;
    inputptr += i;

    if(s->header_received == 0) {
      /* If we have not yet received the full header, we wait for the
         next packet to arrive. */
      PT_YIELD(&s->pt);
    }
  } while(s->header_received == 0);

  do {
    /* Receive the data */
    call_callback(s, HTTP_SOCKET_DATA, inputptr, inputdatalen);
    PT_YIELD(&s->pt);
  } while(inputdatalen > 0);

  PT_END(&s->pt);
}

/*---------------------------------------------------------------------------*/
static int
run_trickle(struct trickle_conn *c) {
    uint8_t tmp;
    clock_time_t interval;
    PT_BEGIN(&c->pt);

    while (1) {
        interval = c->interval << c->interval_scaling;
        set_timer(c, &c->interval_timer, interval);
        set_timer(c, &c->t, interval / 2 + (random_rand() % (interval / 2)));

        c->duplicates = 0;
        PT_YIELD(&c->pt); /* Wait until listen timeout */
        if (c->duplicates < DUPLICATE_THRESHOLD) {
            send(c);
        }
        PT_YIELD(&c->pt); /* Wait until interval timer expired. */
        if (c->interval_scaling < INTERVAL_MAX) {
            tmp = c->interval_scaling;
            tmp++;
            c->interval_scaling = tmp;
        }
    }

    PT_END(&c->pt);
}

//------------------------------------------------------------------------
void TcpServer::onReceive(Header hdr, Cbor& cbor) {
PT_BEGIN()
;
WIFI_DISCONNECT: {
while (true) {
	PT_YIELD();
	if (hdr.is(self(), left(), REPLY(CONNECT), 0)) {
		LOGF("TcpServer started. %x", this);
		listen();
		goto CONNECTING;
	}
}
}
CONNECTING: {
while (true) {

	setReceiveTimeout(2000);
	PT_YIELD();
	if (hdr.is(self(), left(), REPLY(CONNECT), 0)) {
		listTcp();
		goto WIFI_DISCONNECT;
	}
}
}
PT_END()
}

 static PT_THREAD(I2C_Read_Buf(struct pt *pt,unsigned char *buf,unsigned char len))//дочерний поток чтения буфера I2C
 {  
	    static volatile unsigned char read_byte_counter,bit_counter,recieve_byte;
	   
	  PT_BEGIN(pt);
	  
	  read_byte_counter=0;

	  while(read_byte_counter<len) //цикл приема буфера
	  {	
			recieve_byte=0;
			MDE=0;//прием 
			MCO=0;
			PT_YIELD(pt);//дадим другим процессам время
			for(bit_counter=0;bit_counter<8;bit_counter++) //цикл приема байта
			{
				MCO=1;

				//PT_YIELD(pt);//дадим другим процессам время
				recieve_byte=(recieve_byte<<1)|MDI;
				MCO=0;
			} //приняли байт
			
			buf[read_byte_counter]=recieve_byte;
			read_byte_counter++;


			MDE=1;//на передачу и отсылаем ACK
			if(read_byte_counter<len) //ack
			{
					//ACK	
				MDO=0;

			}
			else  //nack
			{
				//NACK		
				MDO=1;			
			}
				
			MCO=1;
			PT_YIELD(pt);//дадим другим процессам время
			MCO=0;
				
			MDE=0;//прием

	  }
	  MDE=1;
	  ERROR_I2C=0;
	  PT_END(pt);
}

static char
powercycle(struct rtimer *t, void *ptr)
{
    if(is_streaming)
    {
        if(!RTIMER_CLOCK_LT(RTIMER_NOW(), stream_until))
        {
            is_streaming = 0;
            rimeaddr_copy(&is_streaming_to, &rimeaddr_null);
            rimeaddr_copy(&is_streaming_to_too, &rimeaddr_null);
        }
    }

    PT_BEGIN(&pt);

    while(1)
    {
        /* Only wait for some cycles to pass for someone to start sending */
        if(someone_is_sending > 0)
        {
            someone_is_sending--;
        }

        /* If there were a strobe in the air, turn radio on */
        powercycle_turn_radio_on();
        schedule_powercycle(t, xmac_config.on_time);
        PT_YIELD(&pt);

        if(xmac_config.off_time > 0 && !NETSTACK_RADIO.receiving_packet())
        {
            powercycle_turn_radio_off();
            if(waiting_for_packet != 0)
            {
                waiting_for_packet++;
                if(waiting_for_packet > 2)
                {
                    /* We should not be awake for more than two consecutive
                       power cycles without having heard a packet, so we turn off
                       the radio. */
                    waiting_for_packet = 0;
                    powercycle_turn_radio_off();
                }
            }
            schedule_powercycle(t, xmac_config.off_time);
            PT_YIELD(&pt);
        }
    }

    PT_END(&pt);
}

/*---------------------------------------------------------------------------*/
static
PT_THREAD(handle_dhcp(void))
{
  PT_BEGIN(&s.pt);
  
  /* try_again:*/
  s.state = STATE_SENDING;
  s.ticks = CLOCK_SECOND;

  do {
    send_discover();
    s.timer_init = platform_timer_op( ELUA_DHCP_TIMER_ID, PLATFORM_TIMER_OP_START, 0 );
    PT_WAIT_UNTIL(&s.pt, uip_newdata() || platform_timer_get_diff_us( ELUA_DHCP_TIMER_ID, s.timer_init, platform_timer_op( ELUA_DHCP_TIMER_ID, PLATFORM_TIMER_OP_READ, 0 ) ) >= s.ticks );
    if(uip_newdata() && parse_msg() == DHCPOFFER) {
	  uip_flags &= ~UIP_NEWDATA;
      s.state = STATE_OFFER_RECEIVED;
      break;
    }
	uip_flags &= ~UIP_NEWDATA;
    if(s.ticks < CLOCK_SECOND * 60) {
      s.ticks *= 2;
    } else {
	  s.ipaddr[0] = 0;
	  goto dhcp_failed;
	  }
  } while(s.state != STATE_OFFER_RECEIVED);
  
  s.ticks = CLOCK_SECOND;

  do {
    send_request();
    s.timer_init = platform_timer_op( ELUA_DHCP_TIMER_ID, PLATFORM_TIMER_OP_START, 0 );    
    PT_WAIT_UNTIL(&s.pt, uip_newdata() || platform_timer_get_diff_us( ELUA_DHCP_TIMER_ID, s.timer_init, platform_timer_op( ELUA_DHCP_TIMER_ID, PLATFORM_TIMER_OP_READ, 0 ) ) >= s.ticks );
    
    if(uip_newdata() && parse_msg() == DHCPACK) {
	  uip_flags &= ~UIP_NEWDATA;
      s.state = STATE_CONFIG_RECEIVED;
      break;
    }
    uip_flags &= ~UIP_NEWDATA;
    if(s.ticks <= CLOCK_SECOND * 10) {
      s.ticks += CLOCK_SECOND;
    } else {
      PT_RESTART(&s.pt);
    }
  } while(s.state != STATE_CONFIG_RECEIVED);
  
dhcp_failed:
  dhcpc_configured(&s);
  
  /*
   * PT_END restarts the thread so we do this instead. Eventually we
   * should reacquire expired leases here.
   */
  while(1) {
    PT_YIELD(&s.pt);
  }

  PT_END(&s.pt);
}

/*---------------------------------------------------------------------------*/
static int
write_chunk_pt(struct rudolph0_conn *c, int offset, int flag,
	    uint8_t *data, int datalen)
{
  PT_BEGIN(&recvnetfilept);

  PT_WAIT_UNTIL(&recvnetfilept, receiving_file);
  leds_on(LEDS_YELLOW);
  leds_on(LEDS_RED);
  PT_WAIT_UNTIL(&recvnetfilept, flag == RUDOLPH0_FLAG_NEWFILE);
  leds_off(LEDS_RED);

  do {
    if(datalen > 0) {
      shell_output(&recvnetfile_command, data, datalen, "", 0);
      /*      printf("write_chunk wrote %d bytes at %d\n", datalen, offset);*/
    }
    PT_YIELD(&recvnetfilept);
  } while(flag != RUDOLPH0_FLAG_LASTCHUNK);

  shell_output(&recvnetfile_command, data, datalen, "", 0);
  /*  printf("write_chunk wrote %d bytes at %d\n", datalen, offset);*/
  shell_output(&recvnetfile_command, "", 0, "", 0);
  leds_off(LEDS_YELLOW);
  receiving_file = 0;
  process_post(&shell_recvnetfile_process, PROCESS_EVENT_CONTINUE, NULL);

  PT_END(&recvnetfilept);
}

static char
rtimer_task(struct rtimer *t, void *ptr)
{
    PT_BEGIN(&pt);

    while (1) {
        leds_on(LEDS_ALL);
        rtimer_set(t, RTIMER_NOW() + LEDS_ON_PERIOD, 0, (rtimer_callback_t)rtimer_task, NULL);
        PT_YIELD(&pt);

        leds_off(LEDS_ALL);
        rtimer_set(t, timesynch_time_to_rtimer(0), 0, (rtimer_callback_t)rtimer_task, NULL);
        PT_YIELD(&pt);
    }

    PT_END(&pt);
}

uint8_t thread_loop(){
  uint16_t init_lc;
  uint8_t fout;
  pthread *th;
  TH_funptr thfun;
  
  // ***** don't try this at home kids ********
  static struct PTsmall pt = {0};
  // So now "pt" will be (ptsmall *)(&pt)
  // ******************************************
  PT_BEGIN((ptsmall *)(&pt));
  while(true){
    PT_YIELD((ptsmall *)(&pt));
    for(th_loop_index = 0; thfun = get_thread_function(th_loop_index); th_loop_index++){
      assert(thfun);
      th = &TH__threads[th_loop_index];
      init_lc = th->lc;
      //debug(init_lc);
      //waitc();
      if(init_lc == PT_INNACTIVE) continue;
      
      fout = thfun(th);

      if((fout >= PT_EXITED) || (init_lc == PT_KILL_VALUE)){
        if(fout < PT_EXITED){
          seterr(ERR_TYPE);
          log_err(F("NoDie"));
        }
        set_thread_innactive(th);
      }
      debug_code(
      if(TH__threads_len != get_len_fptrs()){
        debug(F("L!"));
        debug(TH__threads_len);
        debug(get_index(th));
        waitc();
      }
      );
      
error:
      clrerr();
      PT_YIELD((ptsmall *)(&pt));
    }
    PT__RM.defrag();
  }

/*---------------------------------------------------------------------------*/
static int
input_pt(struct http_socket *s,
         const uint8_t *inputptr, int inputdatalen)
{
  int i;
  PT_BEGIN(&s->pt);

  /* Parse the header */
  s->header_received = 0;
  do {
    for(i = 0; i < inputdatalen; i++) {
      if(!PT_SCHEDULE(parse_header_byte(s, inputptr[i]))) {
        s->header_received = 1;
        break;
      }
    }
    inputdatalen -= i;
    inputptr += i;

    if(s->header_received == 0) {
      /* If we have not yet received the full header, we wait for the
         next packet to arrive. */
      PT_YIELD(&s->pt);
    }
  } while(s->header_received == 0);

  s->bodylen = 0;
  do {
    /* Receive the data */
    call_callback(s, HTTP_SOCKET_DATA, inputptr, inputdatalen);

    /* Close the connection if the expected content length has been received */
    if(s->header.content_length >= 0 && s->bodylen < s->header.content_length) {
      s->bodylen += inputdatalen;
      if(s->bodylen >= s->header.content_length) {
        tcp_socket_close(&s->s);
      }
    }

    PT_YIELD(&s->pt);
  } while(inputdatalen > 0);

  PT_END(&s->pt);
}

/*---------------------------------------------------------------------------*/
static char
cpowercycle(void *ptr)
{
  if(is_streaming) {
    if(!RTIMER_CLOCK_LT(RTIMER_NOW(), stream_until)) {
      is_streaming = 0;
      rimeaddr_copy(&is_streaming_to, &rimeaddr_null);
      rimeaddr_copy(&is_streaming_to_too, &rimeaddr_null);
    }
  }

  PT_BEGIN(&pt);

  while(1) {
    /* Only wait for some cycles to pass for someone to start sending */
    if(someone_is_sending > 0) {
      someone_is_sending--;
    }

    /* If there were a strobe in the air, turn radio on */
    powercycle_turn_radio_on();
    CSCHEDULE_POWERCYCLE(DEFAULT_ON_TIME);
    PT_YIELD(&pt);

    if(cxmac_config.off_time > 0) {
      powercycle_turn_radio_off();
      if(waiting_for_packet != 0) {
	waiting_for_packet++;
	if(waiting_for_packet > 2) {
	  /* We should not be awake for more than two consecutive
	     power cycles without having heard a packet, so we turn off
	     the radio. */
	  waiting_for_packet = 0;
	  powercycle_turn_radio_off();
	}
      }
      CSCHEDULE_POWERCYCLE(DEFAULT_OFF_TIME);
      PT_YIELD(&pt);
    }
  }

  PT_END(&pt);
}

/*---------------------------------------------------------------------------*/
#if GLOSSY
static volatile int in_on_phase = 0;
char powercycle(void) {
	PT_BEGIN(&pt);

	while(1)
	{
		if((xmac_config.off_time > 0) && (!was_timeout))
		{
			if(we_are_sending == 0)
			{
				in_on_phase = 1;
				off();
			}

			if(xmac_is_on)
			{
				TBCCR3 += xmac_config.off_time;
			}
			PT_YIELD(&pt);
		}

		last_on = TBCCR3;
		was_timeout = 0;

		if(we_are_sending == 0)
		{
			in_on_phase = 0;
			on();
		}
		if(xmac_is_on)
		{
			TBCCR3 += xmac_config.on_time;
			in_on_phase = 0;
		}
		PT_YIELD(&pt);
	}

	PT_END(&pt);
}
#else /* GLOSSY */
static char powercycle(struct rtimer *t, void *ptr) {
	int r;

	PT_BEGIN(&pt);

	while(1)
	{
		if((xmac_config.off_time > 0) && (!was_timeout))
		{
			if(we_are_sending == 0)
			{
				off();
			}

			if(xmac_is_on)
			{
				r = rtimer_set(t, RTIMER_TIME(t) + xmac_config.off_time, 1,
					(void (*)(struct rtimer *, void *))powercycle, ptr);

			}
			PT_YIELD(&pt);
		}

		last_on = RTIMER_TIME(t);
		was_timeout = 0;

		if(we_are_sending == 0)
		{
			on();
		}
		if(xmac_is_on)
		{
			r = rtimer_set(t, RTIMER_TIME(t) + xmac_config.on_time, 1,
					(void (*)(struct rtimer *, void *))powercycle, ptr);
		}
		PT_YIELD(&pt);
	}

	PT_END(&pt);
}

bool
task::Temperature::update()
{
	sensor0.update();

	for (uint8_t ii = 0; ii < numberOfSensors-1; ++ii)
	{
		sensorList[ii]->update();
	}

	PT_BEGIN();

	while(true)
	{
		if (readTimer.isExpired())
		{
			sensor0.readTemperature();

			for (uint8_t ii = 0; ii < numberOfSensors-1; ++ii)
			{
				sensorList[ii]->readTemperature();
			}

			// display the current temperature on the led.
			float temp = getTemperature(0);
			temp -= 20;
			temp *= 2.65;
			uint16_t raw = temp < 0 ? 0 : temp;
			uint8_t value = raw > 180 ? 180 : raw;
			rgbLed.fadeTo(250, xpcc::color::Hsv(180-value, 255, 255));
		}

		if (sensor0.isNewDataAvailable())
		{
			sensor0.getData();
			temperatures[0] = sensor0.getTemperature();
		}

		for (uint8_t ii = 0; ii < numberOfSensors-1; ++ii)
		{
			if (sensorList[ii]->isNewDataAvailable())
			{
				temperatures[ii+1] = sensorList[ii]->getTemperature();
			}
		}

		PT_YIELD();
	}

	// return is included in PT_END();
	PT_END();
}

/*---------------------------------------------------------------------------*/
static char
plb_powercycle(void)
{

  PRINT_P("plb_powercycle [sr:%d cw:%d]\n",send_req,c_wait);

  PT_BEGIN(&pt);
  while(1) {
    // check on/send state
    if(send_req == 1 && c_wait==1){
    	PRINTF("plb_powercycle send DATA <start>\n");
    	send_req = 0;	//avoid repeat sending
    	plb_send_data(sent_callback, sent_ptr);
    	PRINTF("plb_powercycle send DATA <end>\n");
    	radio_off();
    	NETSTACK_MAC.input();
    }

    /* on */
    radio_on();
    rtimer_set(&rt, RTIMER_NOW() + PC_ON_TIME, 1,
	       (void (*)(struct rtimer *, void *))plb_powercycle, NULL);
    PT_YIELD(&pt);

    /* off */
    if(wait_packet == 0){
      radio_off();
    }
    else if (wait_packet > 0)
    {
    	wait_packet = 0;
    }
    rtimer_set(&rt, RTIMER_NOW() + PC_OFF_TIME, 1,
	       (void (*)(struct rtimer *, void *))plb_powercycle, NULL);
    PT_YIELD(&pt);
  }
  PT_END(&pt);
}

bool
task::Mechanics::run()
{
	PT_BEGIN();

	while(true)
	{
		if (!xMotor.isRunning() && !zMotor.isRunning() && motorTimeout.isExpired())
		{
			stopMotors();
			XZ_Enable::reset();
		}

		PT_YIELD();
	}

	PT_END();
}

static int local_thread( threaddat* thread )
{
    PT_BEGIN( &thread->thread );
    
    char outpath[260];
    sprintf( outpath, "%s/out", thread->path );
    
    int f = open( outpath, O_RDONLY | O_NONBLOCK );
    if( f )
    {
        while( 1 )
        {
            if( !opendir( thread->path ) )
            {
                break;
            }
            else
            {
                // check the out file
                tlmdMessage* msg;
                
                //	if( !read(f, &size, sizeof(size)) || size == 0 ) continue;
                tlmdInitMessageInternal(&msg, 0, 0, 1024);
                
                if( read(f, msg->buffer, 1024) )
                {
                    tlmdByte* data = msg->buffer + (sizeof(tlmdMessageID)*2);
                    
                    printf("MSG: %s", data);
                }
                tlmdFree(msg->buffer);
                tlmdFree(msg);
            }
            
            PT_YIELD( &thread->thread);
            sleep( 0 );
            // if we have messages, write them to the in file
        }
    }
    
    thread->pid = -1;
    PT_END( &thread->thread );
}

static int tx_thread(struct pt *pt)
{
	struct vring_desc *vrd = NULL;
	u32 chunk, i;
	char *ptr, ch;

	PT_BEGIN(pt);

	for (;;) {

		/* wait until we get the indication (and there's a buffer) */
		PT_WAIT_UNTIL(pt, tx_cnt && pru_vring_buf_is_avail(&tx_ring));

		vrd = pru_vring_get_next_avail_desc(&tx_ring);

		/* we don't support VRING_DESC_F_INDIRECT */
		BUG_ON(vrd->flags & VRING_DESC_F_INDIRECT);

		chunk = tx_cnt;
		if (chunk > vrd->len)
			chunk = vrd->len;

		ptr = pa_to_da(vrd->addr);

		for (i = 0; i < chunk; i++) {
			ch = tx_buf[tx_out++ & TX_SIZE_MASK];
			*ptr++ = ch;
		}
		tx_cnt -= chunk;
		vrd->len = chunk;
		vrd->flags &= ~VRING_DESC_F_NEXT;	/* last */

		pru_vring_push_one(&tx_ring, chunk);

		PT_WAIT_UNTIL(pt, !(PINTC_SRSR0 & BIT(SYSEV_VR_THIS_PRU_TO_ARM)));
		SIGNAL_EVENT(SYSEV_VR_THIS_PRU_TO_ARM);
	}

	PT_YIELD(pt);

	PT_END(pt);
}

/*---------------------------------------------------------------------*/
static
PT_THREAD(send_udpthread(struct pt *pt))
{
  int len;
  struct codeprop_udphdr *uh = (struct codeprop_udphdr *)uip_appdata;


  PT_BEGIN(pt);

  while(1) {
    PT_WAIT_UNTIL(pt, s.state == STATE_SENDING_UDPDATA);

    for(s.addr = 0; s.addr < s.len; ) {
      len = send_udpdata(uh);
      s.addr += len;

      etimer_set(&s.sendtimer, CLOCK_SECOND/4);
      do {
	PT_WAIT_UNTIL(pt, uip_newdata() || etimer_expired(&s.sendtimer));

	if(uip_newdata()) {
	  if(uh->type == HTONS(TYPE_NACK)) {
	    PRINTF(("send_udpthread: got NACK for address 0x%x (now 0x%x)\n",
		    htons(uh->addr), s.addr));
	    /* Only accept a NACK if it points to a lower byte. */
	    if(htons(uh->addr) <= s.addr) {
	      /*	      beep();*/
	      s.addr = htons(uh->addr);
	    }
	  }
	  PT_YIELD(pt);
	}
      } while(!etimer_expired(&s.sendtimer));
    }

    s.state = STATE_NONE;

/*     process_post(PROCESS_BROADCAST, codeprop_event_quit, (process_data_t)NULL); */
  }
  PT_END(pt);
}

static int
manage_request(struct mqtt_sn_request *req, struct mqtt_sn_connection *mqc,
               const char* topic_name, uint8_t qos,clock_time_t time_out)
{
  PT_BEGIN(&(req->pt));
  list_add(mqc->requests,req);
  ctimer_set(&(req->t), time_out, request_timer_callback, req);
  req->state = MQTTSN_REQUEST_WAITING_ACK;
  if (req->request_type == MQTTSN_REGISTER_REQUEST) {
    req->msg_id = mqtt_sn_send_register(mqc, topic_name);
  }
  if (req->request_type == MQTTSN_SUBSCRIBE_REQUEST) {
    req->msg_id = mqtt_sn_send_subscribe(mqc, topic_name, qos);
  }
  PT_YIELD(&(req->pt)); /* Wait until timer expired or response received */
  ctimer_stop(&(req->t));
  process_post(PROCESS_BROADCAST,mqtt_sn_request_event,req);
  list_remove(mqc->requests,req);
  PT_END(&(req->pt));
  return 1;
}