#include "rca.h"
#include "rca_lib.h"

extern "C" {
extern void yield(void);
extern void rca_ena_int(uint_t intno);
extern void rca_dis_int(uint_t intno);
}

#ifdef RCA_ENABLE_PROFILING
volatile uint32_t rca_idle_result;
volatile uint32_t rca_isr_result;
volatile uint32_t rca_dispatch_result;
volatile uint32_t rca_timer_isr_result;
volatile uint32_t rca_usb_isr_result;
volatile uint32_t rca_sercom0_isr_result;
volatile uint32_t rca_sercom2_isr_result;
volatile uint32_t rca_sercom3_isr_result;
volatile uint32_t rca_sercom4_isr_result;
volatile uint32_t rca_sercom5_isr_result;
volatile uint32_t rca_eic_isr_result;
volatile uint32_t rca_tc5_isr_result;
volatile uint32_t rca_rtc_isr_result;

static volatile uint32_t rca_idle_cnt;
static volatile uint32_t rca_isr_cnt;
static volatile uint32_t rca_dispatch_cnt;
static volatile uint32_t rca_timer_isr_cnt;
static volatile uint32_t rca_usb_isr_cnt;
static volatile uint32_t rca_sercom0_isr_cnt;
static volatile uint32_t rca_sercom2_isr_cnt;
static volatile uint32_t rca_sercom3_isr_cnt;
static volatile uint32_t rca_sercom4_isr_cnt;
static volatile uint32_t rca_sercom5_isr_cnt;
static volatile uint32_t rca_eic_isr_cnt;
static volatile uint32_t rca_tc5_isr_cnt;
static volatile uint32_t rca_rtc_isr_cnt;     
static volatile uint32_t rca_profiling_cyccnt;
#endif /* RCA_ENABLE_PROFILING */

Inline void
rca_di(void){
	Asm("cpsid f":::"memory");
}

/*
 *  FAULTMASK‚̃NƒŠƒA
 */
Inline void
rca_ei(void){
	Asm("cpsie f":::"memory");
}

void
rca_init(intptr_t exinf)
{
    init(); //wiring.c
}

void
rca_ena_int(uint_t intno) {
    if (ena_int(intno + 16) != E_OK) {
        while(1); //ToDo
    }

}

void
rca_dis_int(uint_t intno) {
    if (dis_int(intno + 16) != E_OK) {
        while(1); //ToDo
    }
}

extern const DeviceVectors exception_table;

#ifdef RCA_RR_SCEDULE
static uint32_t time_slice = 0;
#endif /* RCA_RR_SCEDULE */

/*
 *  Cyclic Handler
 */
void
rca_CycHandler(intptr_t exinf)
{    
#ifdef RCA_RR_SCEDULE
    int i;
    if (++time_slice == RCA_RR_SCEDULE_CYCLE) {
        time_slice = 0;
        for(i = 1; i < 16; i++){
            if (RCA_RR_SCEDULE & (i << i)) {
                irot_rdq(i);
            }
        }
    }
#endif /* RCA_RR_SCEDULE */
#ifdef RCA_ENABLE_PROFILING    
    rca_di();
    rca_isr_cnt++;
    rca_timer_isr_cnt++;
    rca_profiling_cyccnt++;
    if(rca_profiling_cyccnt == RCA_PROFILING_CYC_MS){
        rca_idle_result=        rca_idle_cnt;
        rca_isr_result=         rca_isr_cnt;
        rca_dispatch_result=    rca_dispatch_cnt;
        rca_timer_isr_result=   rca_timer_isr_cnt;
        rca_usb_isr_result=     rca_usb_isr_cnt;
        rca_sercom0_isr_result= rca_sercom0_isr_cnt;
        rca_sercom2_isr_result= rca_sercom2_isr_cnt;        
        rca_sercom3_isr_result= rca_sercom3_isr_cnt;
        rca_sercom4_isr_result= rca_sercom4_isr_cnt;
        rca_sercom5_isr_result= rca_sercom5_isr_cnt;
        rca_eic_isr_result=     rca_eic_isr_cnt;
        rca_tc5_isr_result=     rca_tc5_isr_cnt;
        rca_rtc_isr_result=     rca_rtc_isr_cnt;             
        rca_idle_cnt = 0;
        rca_isr_cnt = 0;
        rca_dispatch_cnt = 0;
        rca_timer_isr_cnt = 0;
        rca_usb_isr_cnt = 0;
        rca_sercom0_isr_cnt = 0;
        rca_sercom2_isr_cnt = 0;        
        rca_sercom3_isr_cnt = 0;
        rca_sercom4_isr_cnt = 0;
        rca_sercom5_isr_cnt = 0;
        rca_eic_isr_cnt = 0;
        rca_tc5_isr_cnt = 0;
        rca_rtc_isr_cnt = 0;
        rca_profiling_cyccnt = 0;
    }
    rca_ei();
#endif /* RCA_ENABLE_PROFILING */    
    ((void(*)(void))(exception_table.pfnSysTick_Handler))();        
}

/*
 *  Interrupt Handler
 */
void
rca_USB_Handler(void)
{
#ifdef RCA_ENABLE_PROFILING
    rca_di();
    rca_usb_isr_cnt++;
    rca_isr_cnt++;
    rca_ei();
#endif /* RCA_ENABLE_PROFILING */    
    ((void(*)(void))(exception_table.pfnUSB_Handler))();
}

void
rca_SERCOM0_Handler(void)
{
#ifdef RCA_ENABLE_PROFILING
    rca_di();
    rca_sercom0_isr_cnt++;
    rca_isr_cnt++;
    rca_ei();
#endif /* RCA_ENABLE_PROFILING */    
    ((void(*)(void))(exception_table.pfnSERCOM0_Handler))();
}

#ifdef RCA_USE_SERIAL3
#define PIN_SERIAL3_RX 49  /* D5 */
#define PIN_SERIAL3_TX 48  /* D4 */

Uart Serial3(&sercom2, PIN_SERIAL3_RX, PIN_SERIAL3_TX);

void
rca_SERCOM2_Handler(void)
{
#ifdef RCA_ENABLE_PROFILING
    rca_di();
    rca_sercom2_isr_cnt++;
    rca_isr_cnt++;
    rca_ei();
#endif /* RCA_ENABLE_PROFILING */
    Serial3.IrqHandler();
}
#endif /* RCA_USE_SERIAL3 */

void
rca_SERCOM3_Handler(void)
{
#ifdef RCA_ENABLE_PROFILING
    rca_di();
    rca_sercom3_isr_cnt++;
    rca_isr_cnt++;
    rca_ei();
#endif /* RCA_ENABLE_PROFILING */    
    ((void(*)(void))(exception_table.pfnSERCOM3_Handler))();
}

void
rca_SERCOM4_Handler(void)
{
#ifdef RCA_ENABLE_PROFILING
    rca_di();
    rca_sercom4_isr_cnt++;
    rca_isr_cnt++;
    rca_ei();
#endif /* RCA_ENABLE_PROFILING */    
    ((void(*)(void))(exception_table.pfnSERCOM4_Handler))();
}

#ifdef TOPPERS_USE_ARDUINO_SERIAL
void
rca_SERCOM5_Handler(void)
{
#ifdef RCA_ENABLE_PROFILING
    rca_di();
    rca_sercom5_isr_cnt++;
    rca_isr_cnt++;
    rca_ei();
#endif /* RCA_ENABLE_PROFILING */    
    ((void(*)(void))(exception_table.pfnSERCOM5_Handler))();
}
#endif /* TOPPERS_USE_ARDUINO_SERIAL */

void
rca_EIC_Handler(void)
{
#ifdef RCA_ENABLE_PROFILING
    rca_di();
    rca_eic_isr_cnt++;
    rca_isr_cnt++;
    rca_ei();
#endif /* RCA_ENABLE_PROFILING */    
    ((void(*)(void))(exception_table.pfnEIC_Handler))();
}

void
rca_TC5_Handler(void)
{
#ifdef RCA_ENABLE_PROFILING
    rca_di();
    rca_tc5_isr_cnt++;
    rca_isr_cnt++;
    rca_ei();
#endif /* RCA_ENABLE_PROFILING */    
    ((void(*)(void))(exception_table.pfnTC5_Handler))();
}

void
rca_RTC_Handler(void)
{
#ifdef RCA_ENABLE_PROFILING
    rca_di();
    rca_rtc_isr_cnt++;
    rca_isr_cnt++;
    rca_ei();
#endif /* RCA_ENABLE_PROFILING */
    ((void(*)(void))(exception_table.pfnRTC_Handler))();
}

/*
 *  For delay
 */
void
yield(void){
#ifdef RCA_RR_SCEDULE    
    time_slice = 0;
#endif /* RCA_RR_SCEDULE */
#ifdef RCA_ENABLE_PROFILING
    rca_di();
    rca_dispatch_cnt++;
    rca_ei();
#endif /* RCA_ENABLE_PROFILING */    
    dly_tsk(0);
}

extern void setup(void);
extern void loop(void);

void
rca_maintask(intptr_t exinf)
{
    syslog(LOG_NOTICE, "Arduino Main Task start!");
    dly_tsk(1);
#ifdef USBCON
    USBDevice.init();
    USBDevice.attach();
#endif /* USBCON */

    setup();

    chg_pri(0, RCA_MAINTASK_LOOP_PRI);
    
    while(1){
        loop();
        if (serialEventRun) serialEventRun();
    }    
}

#define RCA_TASK_BODY(NUM) \
extern void task##NUM##_setup(void); \
extern void task##NUM##_loop(void); \
 \
void \
rca_task##NUM(intptr_t exinf) \
{ \
    syslog(LOG_NOTICE, "Arduino Task" #NUM " start!"); \
    dly_tsk(1); \
 \
    task##NUM##_setup(); \
     \
    chg_pri(0, RCA_TASK##NUM##_LOOP_PRI); \
         \
    while(1){ \
        task##NUM##_loop(); \
    }     \
}

#if RCA_NUM_TASK > 0
RCA_TASK_BODY(1)
#endif /* RCA_NUM_TASK > 0 */

#if RCA_NUM_TASK > 1
RCA_TASK_BODY(2)
#endif /* RCA_NUM_TASK > 1 */

#if RCA_NUM_TASK > 2
RCA_TASK_BODY(3)
#endif /* RCA_NUM_TASK > 2 */

#if RCA_NUM_TASK > 3
RCA_TASK_BODY(4)
#endif /* RCA_NUM_TASK > 3 */

#if RCA_NUM_TASK > 4
RCA_TASK_BODY(5)
#endif /* RCA_NUM_TASK > 4 */

#ifdef RCA_ENABLE_PROFILING
void
rca_idle_task(intptr_t exinf) {
    while(1){
        rca_di();
        rca_idle_cnt++;
        rca_ei();
    }
}
#endif /* RCA_ENABLE_PROFILING */