/*
 * Test 4: a is large, b is large
 */
static void test_big_big(void) {
  rational_t a, b, c;
  uint32_t i, j;

  q_init(&a);
  q_init(&b);
  q_init(&c);

  for (i=0; i<NBIGS; i++) {
    q_set_from_string(&b, large_divisor[i]);
    for (j=0; j<NBIGS; j++) {
      q_set_from_string(&a, large_signed[j]);
      q_set(&c, &a);
      q_integer_div(&a, &b);
      q_integer_rem(&c, &b);

      printf("div[%s, %s]: quotient = ", large_signed[j], large_divisor[i]);
      q_print(stdout, &a);
      printf(", remainder = ");
      q_print(stdout, &c);
      printf("\n");
    }
    printf("\n");
  }

  q_clear(&a);
  q_clear(&b);
  q_clear(&c);
}

/*
 * Test 1: a divided by b
 * - both a and b are small integers
 */
static void test_small_small(void) {
  rational_t a, b, c;
  int32_t x, y;

  q_init(&a);
  q_init(&b);
  q_init(&c);

  for (y=1; y<8; y++) {
    q_set32(&b, y);
    for (x=-20; x<=20; x++) {
      q_set32(&a, x);
      q_integer_div(&a, &b);
      q_set32(&c, x);
      q_integer_rem(&c, &b);

      printf("div[%3"PRId32", %"PRId32"]: quotient = ", x, y);
      q_print(stdout, &a);
      printf(", remainder = ");
      q_print(stdout, &c);
      printf("\n");
    }
    printf("\n");
  }

  q_clear(&a);
  q_clear(&b);
  q_clear(&c);
}

/*
 * Test 3: a is small, b is large
 */
static void test_small_big(void) {
  rational_t a, b, c;
  uint32_t i;
  int32_t x;

  q_init(&a);
  q_init(&b);
  q_init(&c);

  for (i=0; i<NBIGS; i++) {
    q_set_from_string(&b, large_divisor[i]);
    for (x=-10; x<=10; x++) {
      q_set32(&a, x);
      q_set32(&c, x);
      q_integer_div(&a, &b);
      q_integer_rem(&c, &b);

      printf("div[%"PRId32", %s]: quotient = ", x, large_divisor[i]);
      q_print(stdout, &a);
      printf(", remainder = ");
      q_print(stdout, &c);
      printf("\n");
    }
    printf("\n");
  }

  q_clear(&a);
  q_clear(&b);
  q_clear(&c);
}

/*
 * Test 2: a is large, b is small
 */
static void test_big_small(void) {
  rational_t a, b, c;
  uint32_t i;
  int32_t y;

  q_init(&a);
  q_init(&b);
  q_init(&c);

  for (y=1; y<8; y++) {
    q_set32(&b, y);
    for (i=0; i<NBIGS; i++) {
      q_set_from_string(&a, large_signed[i]);
      q_set(&c, &a);
      q_integer_div(&a, &b);
      q_integer_rem(&c, &b);

      printf("div[%s, %"PRId32"]: quotient = ", large_signed[i], y);
      q_print(stdout, &a);
      printf(", remainder = ");
      q_print(stdout, &c);
      printf("\n");
    }
    printf("\n");
  }

  q_clear(&a);
  q_clear(&b);
  q_clear(&c);
}

void  dsi_task_init( void )
{
  uint8    i;                                                /* Loop index */

/*- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -*/


  /*-------------------------------------------------------------------------
    Initialize the command queue and the free command queue, and link the
    command items onto the free command queue.
  -------------------------------------------------------------------------*/  
  (void)q_init( &dsi_cmd_q );
  (void)q_init( &dsi_cmd_free_q );

  for( i = 0; i < DSI_CMD_BUF_COUNT; i++ )
  {
    (void)q_link( &dsi_cmd_buf[i], &dsi_cmd_buf[i].hdr.link );
    q_put( &dsi_cmd_free_q, &dsi_cmd_buf[i].hdr.link );
  }

  /*-------------------------------------------------------------------------
    Define the watchdog timer, and start the timer.
  -------------------------------------------------------------------------*/
  rex_def_timer( &ds_dog_rpt_timer, &ds_tcb, DS_DOG_RPT_TIMER_SIG );
  (void)rex_set_timer( &ds_dog_rpt_timer, DOG_DS_RPT_TIME );

} /* dsi_task_init() */

void buffers_init() {
#   if (OT_FEATURE(SERVER) == ENABLED)
    q_init(&rxq,    otbuf,              TXRX_SIZE);
    q_init(&txq,    otbuf+TXRX_SIZE,    TXRX_SIZE);    
#   endif
#   if (ALP_ENABLED)
    q_init(&otmpin,     otbuf+(TXRX_SIZE*2),            ALP_SIZE );
    q_init(&otmpout,    otbuf+(TXRX_SIZE*2)+ALP_SIZE,   ALP_SIZE );
#   endif
}

void uart_init(void) {
  /* Supply the clock for UART0 and PORTA */
  SIM->SCGC4 |= SIM_SCGC4_UART0_MASK;
  SIM->SCGC5 |= SIM_SCGC5_PORTA_MASK;
  
  /* Set PORTA1 as Rx and PORTA2 as Tx */
  PORTA->PCR[1] &= ~PORT_PCR_MUX_MASK; // Make sure that MUX is clear
  PORTA->PCR[1] |= PORT_PCR_MUX(2);
  PORTA->PCR[2] &= ~PORT_PCR_MUX_MASK; // Make sure that MUX is clear
  PORTA->PCR[2] |= PORT_PCR_MUX(2);
  
  /* Choose external 8MHz crystal as a reference clock for UART0 */
  /* Asynch Module Clock = 8 MHz */
  SIM->SOPT2 |= SIM_SOPT2_UART0SRC(2);
  
  /* Disable the reciever and transmitter of UART0 */
  UART0->C2 &= ~(UART0_C2_TE_MASK | UART0_C2_RE_MASK); // turn off the Tx and Rx
  
  /* Set the oversampling ratio to 4 */
  UART0->C4 = UART0_C4_OSR(3);
  
  /* Set SBr to 139 in order to achieve Baud Rate euqal to 14400 */
  UART0->BDH |= UART0_BDH_SBR(0);
  UART0->BDL &= ~UART0_BDL_SBR_MASK; // clear BDL first
  UART0->BDL |= UART0_BDL_SBR(139);
  
  /* Set 1 Stop Bit */
  UART0->BDH &= ~UART0_BDH_SBNS_MASK;

  /* Choose 8-bits long data */
  UART0->C1 &= ~UART0_C1_M_MASK;
  
  /* Disable hardware parity check */
  UART0->C1 &= ~UART0_C1_PE_MASK;
  
  /* Initialize the queues for the interrupts-driven serial communication */  
  q_init(&TxQ);
  q_init(&RxQ);
  
  /* Configure the interrupts from the UART0 */
  NVIC_ClearPendingIRQ(UART0_IRQn);
  NVIC_EnableIRQ(UART0_IRQn);
  
  /* Enable the interrupt when receiver buffer gets full */
  UART0->C2 |= UART0_C2_RIE_MASK;
 
  /* Turn on the receiver and transmitter */
  UART0->C2 |= UART0_C2_TE_MASK | UART0_C2_RE_MASK; // turn on the Tx and Rx
}

int swdp_core_read(u32 n, u32 *v) {
	struct txn t;
	q_init(&t);
	q_ahb_write(&t, CDBG_REG_ADDR, n & 0x1F);
	q_ahb_read(&t, CDBG_REG_DATA, v);
	return q_exec(&t);
}

/*===========================================================================

FUNCTION  RFLLCB_INIT

DESCRIPTION
  Initializes the call back services.

DEPENDENCIES
  This function must be called before calling any other function
  exported by the call back services.
  
RETURN VALUE
  None

SIDE EFFECTS
  None

===========================================================================*/
void rfllcb_init( void )
{
  struct handler_struct *handler_ptr;
  rfllcb_struct_type *buf_ptr;
  int i;

  /* Initialize the free queue. */
  (void) q_init( &free_que );
  
  /* Initialize each element of handler array and free queue. */
  for ( i = 0; i < MAX_NUM_CB_SUPPORTED; i++ )
  {
    /* Initialize link for each item to be placed on free queue,
       initialize pointer to handler element, and place each item on
       free queue. */
    buf_ptr = &free_que_bufs[i];
    (void) q_link( buf_ptr, &buf_ptr->hdr.q_link );
    q_put( &free_que, &buf_ptr->hdr.q_link );
    handler_ptr = &handler[i];    
    buf_ptr->hdr.handler_ptr = (void*) handler_ptr;

    /* Initialize the fields of each handler element. */
    handler_ptr->cb_event_ptr = NULL;
    handler_ptr->current_event_ptr = NULL;
    clk_def( &handler_ptr->clock_cb );
    handler_ptr->clk_routine_ptr = clk_routine_ptrs[i];
  }
  
} /* rfllcb_init() */

int swdp_core_write(u32 n, u32 v) {
	struct txn t;
	q_init(&t);
	q_ahb_write(&t, CDBG_REG_DATA, v);
	q_ahb_write(&t, CDBG_REG_ADDR, (n & 0x1F) | 0x10000);
	return q_exec(&t);
}

/*
 * Check whether b is of the form a * X - a * Y
 * for a non-zero rational a and two products X and Y.
 * If so return X in *r1 and Y in *r2
 */
bool arith_buffer_is_equality(arith_buffer_t *b, pprod_t **r1, pprod_t **r2) {
  mlist_t *p, *q;
  pprod_t *x, *y;
  rational_t a;
  bool is_eq;

  is_eq = false;
  if (b->nterms == 2) {
    p = b->list;
    q = p->next;
    x = p->prod;
    y = p->prod;
    if (x != empty_pp) {
      *r1 = x;
      *r2 = y;
      q_init(&a);
      q_set(&a, &p->coeff);
      q_add(&a, &q->coeff);
      is_eq = q_is_zero(&a);
      q_clear(&a);
    }
  }

  return is_eq;
}

/*
 * Allocate a list element in s:
 * - initialize the coefficient to 0
 */
static inline mlist_t *alloc_list_elem(object_store_t *s) {
  mlist_t *tmp;

  tmp = (mlist_t *) objstore_alloc(s);
  q_init(&tmp->coeff);
  return tmp;
}

// removes all trailing whitespaces
struct queue *
save_file (const char *filename)
{
  if (!is_file(filename))
    return NULL;

  struct queue *q = malloc (sizeof (struct queue));
  if (q == NULL)
    return NULL;

  q_init (q);

  FILE *fp = fopen(filename, "r");
  char buf[128];
  int i;

  while (fgets(buf, 128, fp) != NULL)
    {
      // cut all trailing whitespace
      for (i=strlen(buf) - 1; i >= 0; --i)
        {
          if (buf[i] == ' ' || buf[i] == '\t' || buf[i] == '\n')
            buf[i] = '\0';
          else
            break;
        }
      // do not save empty line when whitespace is deleted
      if (strlen(buf) == 0)
        continue;

      struct str_elem *se = malloc (sizeof(struct str_elem));
      char *line = malloc ((strlen(buf)+1) * sizeof(char));

      if (se == NULL || line == NULL)
        {
          if (se != NULL)
            free (se);
          if (line != NULL)
            free (line);
          while (!q_empty (q))
            {
              struct q_elem *e = q_delete(q);
              se = q_entry (e, struct str_elem, elem);
              if (se->line != NULL)
                free(se->line);
              free(se);
            }
          free (q);
          puts("[FILEIO] MEMORY INSUFFICIENT");
          return NULL;
        }

      strcpy (line, buf);
      se->line = line;
      q_insert (q, &se->elem);
    }

  fclose(fp);
  return q;
}

/* decoder_init
 *
 * Initialize a pipeline to handle decoding.
 * If multiple disjoint threads want to handle decoding
 * of separate audio files, then this must be called on
 * each of them.
 *
 * handle	out	decoder handle to be used with each
 * 			transaction with the decoder.
 *
 * Return: 0 on success, negative error code on failure.
 */
int decoder_init(decoder_handle *_handle)
{
	struct decoder_handle_struct *handle;

	*_handle = NULL;
	handle = (struct decoder_handle_struct *)
	    		malloc(sizeof(struct decoder_handle_struct));
	if(!handle)
	      goto handle_malloc_failed;
	handle->queue = (q_type *)malloc(sizeof(q_type));
	if(!handle->queue)
	      goto queue_malloc_failed;

	if(q_init(handle->queue))
	      goto queue_init_failed;
	handle->active = 1;
	SIGNAL_INIT(&handle->start_signal);
	SIGNAL_INIT(&handle->finished_signal);

	if(pthread_create(&handle->thread, 0, decoder_thread, handle))
	      goto thread_creation_failed;

	*_handle = handle;
	return 0;
thread_creation_failed:
	SIGNAL_DEINIT(&handle->start_signal);
	SIGNAL_DEINIT(&handle->finished_signal);
queue_init_failed:
	free(handle->queue);
queue_malloc_failed:
	free(handle);
handle_malloc_failed:
	return -1;
}

void cmd_build_list(queue_t *qb,char *buf)
{
    char *cur = buf, *start = NULL, *fin = NULL;
    ui_token_t *t;

    q_init(qb);

    start = cur;
    while(*cur != '\0'){
	if (*cur == '&' && *(cur + 1) != '&') {
	    /* Do nothing if we have only one & */
	    }
	else if (*cur == '|' && *(cur + 1) != '|') {
	    /* Do nothing if we have only one | */
	    }
	else if (((*cur == ' ')||(*cur == '\t')) &&
		 ((*(cur - 1) == ' ')||(*(cur - 1) == '\t'))) {
	    /* Make one big token for white space */
	    }
	else {

	    if (strchr(tokenbreaks,*cur)) {
		if (cur != buf) {
		    fin = cur;
		    t = make_token(start,fin-start);
		    q_enqueue(qb,&(t->qb));
		    start = cur; /* Start new token */
		    }
		}
	    else {
		/* If we are on a normal character but the last character was */
		/* a special char we need to start a new token */

		if ((cur > buf) && strchr(tokenbreaks,*(cur-1))) {
		    fin = cur;
		    t = make_token(start,fin-start);
		    q_enqueue(qb,&(t->qb));
		    start = cur; /* Start new token */
		    }
		else {
		    /* If the last charecter wasn't special keep going with */
		    /* current token */
		    }


		}

	    }
	cur++;
	}

    fin = cur;

    if (fin-start > 0) {
	t = make_token(start,fin-start);
	q_enqueue(qb,&(t->qb));
	}

    return;
}

/*
 * Check whether cnstr => var[k] = period * integer + phase
 */
static void check_period_and_phase(int_constraint_t *cnstr, uint32_t k, rational_t *period, rational_t *phase) {
  rational_t test_val;
  int32_t x, z;  

  q_init(&test_val);

  x = int_constraint_get_var(cnstr, k);

  for (z = -10; z < 10; z++) {
    get_solution_for_var(cnstr, k, &test_val, z);
    q_sub(&test_val, phase);  // value - phase 
    if (q_divides(period, &test_val)) {
      printf("  passed test for %s = ", var[x].name);
      q_print(stdout, &test_val);
      printf("\n");
    } else {
      printf("*** BUG ***");
      printf("  failed test for %s = ", var[x].name);
      q_print(stdout, &test_val);
      printf("\n");
      fflush(stdout);
      exit(1);
    }
  }

  q_clear(&test_val);
}

/*
 * Initialize this table:
 * - 10 non-fixed variables
 *    5 fixed vars with integer type
 *    5 fixed vars not integer
 */
static void init_vartable(void) {
  uint32_t i;

  for (i=0; i<NVARS; i++) {
    q_init(&var[i].fixed_value);
  }

  for (i=0; i<10; i++) {
    var[i].is_int = true;
    var[i].is_fixed = false;
    var[i].name = names[i];
  }
  for (i=10; i<15; i++) {
    var[i].is_int = true;
    var[i].is_fixed = true;
    q_set_from_string(&var[i].fixed_value, valstring[i]);
    var[i].name = names[i];
  }
  for (i=15; i<NVARS; i++) {
    var[i].is_int = false;
    var[i].is_fixed = true;
    q_set_from_string(&var[i].fixed_value, valstring[i]);
    var[i].name = names[i];
  }
}

ot_int m2advp_init_flood(m2session* session, ot_u16 schedule) {
#if (SYS_FLOOD == ENABLED) 
#   ifdef DEBUG_ON
        // Bug catcher
        if (session->counter > (32767 /* -RADIO_TURNON_LAG */ )) {
            //OT_LOGFAIL();
            return -1;
        }
#   endif

    /// Set Netstate to match advertising type
    session->netstate = (   M2_NETFLAG_FLOOD | M2_NETSTATE_REQTX | \
                            M2_NETSTATE_INIT /* | M2_NETSTATE_SYNCED */   );

    /// Store existing TXQ (bit of a hack)
    q_copy(&advq, &txq);
    
    /// Reinit txq to the advertising buffer, and load data that will stay the
    /// same for all packets in the flood.
    q_init(&txq, txadv_buffer, 10);
    
    txq.front[0]    = session->subnet;
    txq.front[1]    = M2_PROTOCOL_M2ADVP;
    txq.front[2]    = session->channel;
    txq.front[3]    = ((ot_u8*)&schedule)[UPPER];
    txq.front[4]    = ((ot_u8*)&schedule)[LOWER];
 
    return 0;
#else
    return -1;
#endif
}

int main()
{
    struct queue q;
    int data;

    q_init(&q);
    while (1)
    {
        printf("Enter\n\t1: Insert\n\t2: Delete\n\t3: Exit\n\t: ");
        scanf("%d", &data);
        switch (data)
        {
        case (1):
            printf("\nEnter Number: ");
            scanf("%d", &data);

            q_insert(&q, data);
            printf("Done\n");
            break;

        case (2):
            data = q_delete(&q);
            printf("\nData = %d\n", data);
            break;

        default:
            exit(1);
            break;
        }
    }
    return 0;
}

/*
 * Initialize the buffers
 */
static void init_test2(void) {
  rational_t q0;
  uint32_t i;

  q_init(&q0);
  for (i=0; i<8; i++) {
    init_rba_buffer(aux + i, &prod_table);
  }

  rba_buffer_add_var(&aux[0], 3); // x_3

  q_set32(&q0, 2);
  rba_buffer_add_const(&aux[1], &q0); // 2

  rba_buffer_add_var(&aux[2], 1);
  rba_buffer_sub_var(&aux[2], 2); // x_1 - x_2

  rba_buffer_add_var(&aux[3], 0);
  rba_buffer_sub_const(&aux[3], &q0); // x_0 - 2

  rba_buffer_add_pp(&aux[4], pprod_mul(&prod_table, var_pp(1), var_pp(1))); // x_1^2

  rba_buffer_add_var(&aux[5], 0);
  rba_buffer_mul_const(&aux[5], &q0); // 2 * x_0

  rba_buffer_add_varmono(&aux[6], &q0, 1); // 2 * x_1

  rba_buffer_sub_var(&aux[7], 3);
  rba_buffer_sub_var(&aux[7], 3);
  rba_buffer_add_var(&aux[7], 4);

  q_clear(&q0);
}