#include <stdlib.h>
#include <stdio.h>
#include <rtems.h>
#include <rtems/error.h>
#include <bsp/gt_timer.h>

#define ISR_INTERVAL    (1000L*133L)    /* 1 msec */
#define DO_NOT_RELOAD   0
#define BUS_SPEED       (133333333L)
#define CLOCK_SPEED     (BUS_SPEED/4)

unsigned long    isr_counter    = 0;
unsigned long    turtle_counter = 0;
unsigned long    rabbit_counter = 0;

unsigned ev_send_turtle, ev_recv_turtle;
unsigned ev_send_rabbit, ev_recv_rabbit;

static rtems_id tid_mont;
static rtems_id tid_turtle;
static rtems_id tid_rabbit;


static char shadow_reg_led = 0x00;
static char *reg_led = 0x9fff0003; 
static char *reg_sw  = 0x9fff0101;

static char turtle_led = 0x01;
static char rabbit_led = 0x01;

static rtems_id procMutex;


double interval_usec(unsigned start, unsigned end)
{
    return (double)(end-start)/(double)CLOCK_SPEED * 1.E+6;
}


static void isr_for_watchdog (void *pVar)
{
    BSP_timer_start(0, ISR_INTERVAL);    /* restart timer */

    isr_counter++;

    if(!(isr_counter%50)) {
        CPU_Get_timebase_low(ev_send_rabbit);
        rtems_event_send(tid_rabbit, RTEMS_EVENT_0);
        rabbit_counter++;
    }
    if(!(isr_counter%200)) {
        CPU_Get_timebase_low(ev_send_turtle);
        rtems_event_send(tid_turtle, RTEMS_EVENT_0);
        turtle_counter++;
    }

}

static rtems_task mont(rtems_task_argument ignored)
{
    for(;;) {
      printf("ISR counter: %10ld, Turtle calls: %10ld, Rabbit calls: %10ld\n", 
             isr_counter, turtle_counter, rabbit_counter);
      printf("Turtle delay: %10.3lf usec, Rabbit delay %10.3lf usec\n",
             interval_usec(ev_send_turtle, ev_recv_turtle),
             interval_usec(ev_send_rabbit, ev_recv_rabbit));

      sleep(2);
    }
}


static rtems_task turtle(rtems_task_argument ignored)
{
    rtems_event_set events;

    printf(">>>> start turtle <<<<\n");

    for(;;) {

        rtems_event_receive(RTEMS_EVENT_0, RTEMS_EVENT_ANY|RTEMS_WAIT,
                            RTEMS_NO_TIMEOUT, &events);

        CPU_Get_timebase_low(ev_recv_turtle);
 
        if(turtle_led == 0x80) turtle_led = 0x01;
        else turtle_led <<= 1;

        rtems_semaphore_obtain(procMutex, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
        shadow_reg_led = turtle_led | rabbit_led; 
        output_byte(reg_led, shadow_reg_led);
        rtems_semaphore_release(procMutex);

    }


    rtems_task_delete(RTEMS_SELF);

}

static rtems_task rabbit(rtems_task_argument ignored)
{

    rtems_event_set events;

    printf(">>>> start rabbit <<<<\n");

    for(;;) {

        rtems_event_receive(RTEMS_EVENT_0, RTEMS_EVENT_ANY|RTEMS_WAIT,
                            RTEMS_NO_TIMEOUT, &events);

        CPU_Get_timebase_low(ev_recv_rabbit);

        if(rabbit_led == 0x80) rabbit_led = 0x01;
        else rabbit_led <<= 1;

        rtems_semaphore_obtain(procMutex, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
        shadow_reg_led = turtle_led | rabbit_led;
        output_byte(reg_led, shadow_reg_led);
        rtems_semaphore_release(procMutex);
    }


    rtems_task_delete(RTEMS_SELF);

}



rtems_task watchdog_tr_init(rtems_task_argument ignored)
{
    printf(">>>> Init for turtle and rabbit <<<<\n");

    rtems_semaphore_create(rtems_build_name('M', 'U', 'T', 'X'),
                           1,
                           RTEMS_FIFO | RTEMS_BINARY_SEMAPHORE | RTEMS_INHERIT_PRIORITY |
                           RTEMS_NO_PRIORITY_CEILING | RTEMS_LOCAL,
                           0,
                           &procMutex);

    rtems_task_create(rtems_build_name('T', 'U', 'R', 'T'),
                      200,
                      16*1024,
                      RTEMS_PREEMPT | RTEMS_TIMESLICE | RTEMS_NO_ASR | RTEMS_INTERRUPT_LEVEL(0),
                      RTEMS_FLOATING_POINT | RTEMS_LOCAL, 
                      &tid_turtle);
    rtems_task_start(tid_turtle, turtle, 0);

    rtems_task_create(rtems_build_name('R', 'A', 'B', 'T'),
                      200,
                      16*1024,
                      RTEMS_PREEMPT | RTEMS_TIMESLICE | RTEMS_NO_ASR | RTEMS_INTERRUPT_LEVEL(0),
                      RTEMS_FLOATING_POINT | RTEMS_LOCAL,
                      &tid_rabbit);
    rtems_task_start(tid_rabbit, rabbit, 0);


    BSP_timer_setup(0, isr_for_watchdog, NULL, DO_NOT_RELOAD);
 
    sleep(1); 


    BSP_timer_start(0, ISR_INTERVAL);

    rtems_task_create(rtems_build_name('M', 'O', 'N', 'T'),
                      200,
                      16*1024,
                      RTEMS_PREEMPT | RTEMS_TIMESLICE | RTEMS_NO_ASR | RTEMS_INTERRUPT_LEVEL(0),
                      RTEMS_FLOATING_POINT | RTEMS_LOCAL,
                      &tid_mont);
    rtems_task_start(tid_mont, mont, 0);



    printf(">>> Finish Init <<<<\n");
}