// need for rand function
////////////////////////////////////

#include "medibot.h"
#include "motors.h"

static int pulse_mode;
static int counter;
static int pwm_control = 40000;
int not_seen_iterations = 0;

rtccTime tm, tm1;
rtccTime t_alarm;

rtccDate dt, dt1;
rtccDate d_alarm;
int turn_time;

BOOL human_found;
BOOL human_found_l;
BOOL human_found_r;


int input_capture_cycles;
int input_capture_cycles_r;
int input_capture_cycles_l;
char curr_day[16];
int curr_hour, curr_min, curr_sec;

days_t days_enum;

__inline__ void update_medicine(void) {
    meds[number_of_meds].day = days_enum;
    meds[number_of_meds].hour = med_hour;
    meds[number_of_meds].minute = med_min;
    meds[number_of_meds].quantity = med_quantity;

    number_of_meds++;
}

__inline__ void marshal_medicine(void) {
    if (0 == strcmp("M", med_day)) {
        days_enum = MONDAY;
        update_medicine();
    } else if (0 == strcmp("Tu", med_day)) {
        days_enum = TUESDAY;
        update_medicine();
    } else if (0 == strcmp("W", med_day)) {
        days_enum = WEDNESDAY;
        update_medicine();
    } else if (0 == strcmp("Th", med_day)) {
        days_enum = THURSDAY;
        update_medicine();
    } else if (0 == strcmp("F", med_day)) {
        days_enum = FRIDAY;
        update_medicine();
    } else if (0 == strcmp("Sat", med_day)) {
        days_enum = SATURDAY;
        update_medicine();
    } else if (0 == strcmp("Sun", med_day)) {
        days_enum = SUNDAY;
        update_medicine();
    } else if (0 == strcmp("done", med_day)) {
        medibot_bluetooth = FOLLOW;
        sort_meds();
        set_first_medicine();
    } else if ((0 == strcmp("del", med_day)) && number_of_meds > 0) {
        number_of_meds--;
    }
}

__inline__ void rc_drive_car(char * rc_drive) {
    if (0 == strcmp("stop", rc_drive)) {
        STOP_CAR();
    } else if (0 == strcmp("fwd", rc_drive)) {
        FWD_CAR(pwm_control, pwm_control_l);
    } else if (0 == strcmp("rev", rc_drive)) {
        REV_CAR(pwm_control, pwm_control_l);
    } else if (0 == strcmp("right", rc_drive)) {
        TURN_RIGHT(pwm_control, pwm_control_l);
    } else if (0 == strcmp("left", rc_drive)) {
        TURN_LEFT(pwm_control, pwm_control_l);
    } else if (0 == strcmp("follow", rc_drive)) {
        medibot_bluetooth = FOLLOW;
        not_seen_iterations = 0;
        STOP_CAR();
    }
}

// === Timer Thread =================================================
// update a 1 second tick counter
char weekday[16];
char alarmday[16];

current_day_to_string(char * day_str) {
    RtccGetTimeDate(&tm1, &dt1);
    if (dt1.wday == MONDAY) {
        sprintf(day_str, "MONDAY");
    } else if (dt1.wday == TUESDAY) {
        sprintf(day_str, "TUESDAY");
    } else if (dt1.wday == WEDNESDAY) {
        sprintf(day_str, "WEDNESDAY");
    } else if (dt1.wday == THURSDAY) {
        sprintf(day_str, "THURSDAY");
    } else if (dt1.wday == FRIDAY) {
        sprintf(day_str, "FRIDAY");
    } else if (dt1.wday == SATURDAY) {
        sprintf(day_str, "SATURDAY");
    } else if (dt1.wday == SUNDAY) {
        sprintf(day_str, "SUNDAY");
    }
}

alarm_day_to_string(char * day_str) {
    //RtccGetTimeDate(&tm1, &dt1);
    if (meds[current_medicine].day == MONDAY) {
        sprintf(day_str, "MONDAY");
    } else if (meds[current_medicine].day == TUESDAY) {
        sprintf(day_str, "TUESDAY");
    } else if (meds[current_medicine].day == WEDNESDAY) {
        sprintf(day_str, "WEDNESDAY");
    } else if (meds[current_medicine].day == THURSDAY) {
        sprintf(day_str, "THURSDAY");
    } else if (meds[current_medicine].day == FRIDAY) {
        sprintf(day_str, "FRIDAY");
    } else if (meds[current_medicine].day == SATURDAY) {
        sprintf(day_str, "SATURDAY");
    } else if (meds[current_medicine].day == SUNDAY) {
        sprintf(day_str, "SUNDAY");
    }
}


set_current_day(char * day_str) {
    dt1.l = RtccGetDate();

    if (0 == strcmp("M", day_str)) {
        dt1.wday = MONDAY;
    }
    else if (0 == strcmp("Tu", day_str)) {
        dt1.wday = TUESDAY;
    }
    else if (0 == strcmp("W", day_str)) {
        dt1.wday = WEDNESDAY;
    }
    else if (0 == strcmp("Th", day_str)) {
        dt1.wday = THURSDAY;
    } else if (0 == strcmp("F", day_str)) {
        dt1.wday = FRIDAY;
    }
    else if (0 == strcmp("Sat", day_str)) {
        dt1.wday = SATURDAY;
    } else if (0 == strcmp("Sun", day_str)) {
        dt1.wday = SUNDAY;
    }

    RtccSetDate(dt1.l);
}

BOOL power_cycle = FALSE;

static PT_THREAD(protothread_timer(struct pt *pt)) {
    PT_BEGIN(pt);
    while (1) {
        power_cycle = TRUE;
        mPORTBClearBits(BIT_3);
        PT_YIELD_TIME_msec(10);
        mPORTBSetBits(BIT_3);
        PT_YIELD_TIME_msec(1);
        input_capture_cycles   = 0;
        input_capture_cycles_l = 0;
        input_capture_cycles_r = 0;

        power_cycle = FALSE;

        // yield time 1 second
        PT_YIELD_TIME_msec(989);

        sys_time_seconds++;
        current_day_to_string(weekday);
        RtccGetTimeDate(&tm1, &dt1);

        tft_fillRoundRect(20, 0, 250, 250, 1, ILI9340_BLACK); // x,y,w,h,radius,color
        sprintf(buffer, "not_seen = %d \n  Name = %s \n  %d:%d:%d\n  %s \n CC = %d\n  HF = %d\n HF_L = %d\n  HF_R = %d\n bluetooth= %d", not_seen_iterations, user_name, bcd2int(tm1.hour), bcd2int(tm1.min), bcd2int(tm1.sec), weekday, input_capture_cycles, human_found, human_found_l, human_found_r, medibot_bluetooth);

//        if ( USER_NAME < medibot_bluetooth ){
//            sprintf(buffer, "  Patient:\n     %s\n", user_name );
            tft_setCursor(20, 100);
            tft_setTextColor(ILI9340_WHITE);
            tft_setTextSize(2);

            tft_writeString(buffer);
//        }
//
//        if (CURRENT_TIME < medibot_bluetooth){
//            sprintf(buffer, "   %d:%d:%d\n    %s \n", bcd2int(tm1.hour), bcd2int(tm1.min), bcd2int(tm1.sec), weekday );
//            tft_setCursor(20, 10);
//            tft_setTextColor(ILI9340_CYAN);
//            tft_setTextSize(3);
//            tft_writeString(buffer);
//        }
//
//        if (MEDICINE_SCHEDULE < medibot_bluetooth && number_of_meds >= 1) {
//            tft_setCursor(20, 200);
//            tft_setTextColor(ILI9340_RED);
//            tft_setTextSize(3);
//            alarm_day_to_string(alarmday);
//            sprintf(buffer, "Next Medicine Time:\n %s\n    %d:%d:%d\n  %d pills", alarmday, meds[current_medicine].hour, meds[current_medicine].minute, meds[current_medicine].quantity);
//            tft_writeString(buffer);
//        }
    } // END WHILE(1)
    PT_END(pt);
} // timer thread

static PT_THREAD(protothread_receive(struct pt *pt)) {
    PT_BEGIN(pt);
    while (1) {
        PT_YIELD_TIME_msec(100);

        // cry, revert to RC mode, spam human for attention
        human_found = FALSE;
        human_found_l = FALSE;
        human_found_r = FALSE;

        if (input_capture_cycles >= 20) {
            human_found = TRUE;
        } else if (input_capture_cycles_l >= 20) {
            human_found_l = TRUE;
        } else if (input_capture_cycles_r >= 20) {
            human_found_r = TRUE;
        }

        input_capture_cycles = 0;
        input_capture_cycles_l = 0;
        input_capture_cycles_r = 0;

    } // END WHILE(1)

    PT_END(pt);
} // thread 3


// =============================================================================
// Input Capture 1 ISR
// =============================================================================
// =============================================================================
// Input Capture 1 ISR
// =============================================================================
// number of cycles for previous input_capture cycles
// ISR Triggered @posedge of input capture pin.
// Saves hardware timer (counter) register to `input_capture_cycles'
// middle 17

void __ISR(_INPUT_CAPTURE_3_VECTOR, ipl3) C3Handler(void) {
    mIC3ReadCapture();

   if (power_cycle == FALSE) {
        input_capture_cycles++;
    }
    // clear the timer interrupt flag
    mIC3ClearIntFlag();
}
// left 18

void __ISR(_INPUT_CAPTURE_2_VECTOR, ipl3) C2Handler(void) {
    mIC2ReadCapture();
   if (power_cycle == FALSE) {
        input_capture_cycles_l++;
    }    // clear the timer interrupt flag
    mIC2ClearIntFlag();
}
// right 24

void __ISR(_INPUT_CAPTURE_1_VECTOR, ipl3) C1Handler(void) {
    mIC1ReadCapture();
   if (power_cycle == FALSE) {
        input_capture_cycles_r++;
    }    // clear the timer interrupt flag    // clear the timer interrupt flag
    mIC1ClearIntFlag();
}


// The serial interface

static PT_THREAD(protothread_serial(struct pt *pt)) {
    PT_BEGIN(pt);
    while (1) {

        if (USER_NAME == medibot_bluetooth) { // GET USER NAME
            // send the prompt via DMA to serial
            if (!errored_out) {
                sprintf(PT_send_buffer, "What is your name?\n");
            } else {
                sprintf(PT_send_buffer, "What was that?\n");
            }

            // by spawning a print thread
            PT_SPAWN(pt, &pt_DMA_output, PT_DMA_PutSerialBuffer(&pt_DMA_output));
            //spawn a thread to handle terminal input
            // the input thread waits for input
            // -- BUT does NOT block other threads
            // string is returned in "PT_term_buffer"
            PT_SPAWN(pt, &pt_input, PT_GetSerialBuffer(&pt_input));
            if (!errored_out) {

                // returns when the thread dies
                // in this case, when <enter> is pushed
                // now parse the string
                sscanf(PT_term_buffer, "%s", user_name);

                medibot_bluetooth = CURRENT_TIME;
            }
        } else if (CURRENT_TIME == medibot_bluetooth) {
            // send the prompt via DMA to serial
            if (!errored_out) {
                sprintf(PT_send_buffer, "Current time? (DAY HR MIN SEC)\n");
            } else {
                sprintf(PT_send_buffer, "What was that?\n");
            }
            // by spawning a print thread
            PT_SPAWN(pt, &pt_DMA_output, PT_DMA_PutSerialBuffer(&pt_DMA_output));

            //spawn a thread to handle terminal input
            // the input thread waits for input
            // -- BUT does NOT block other threads
            // string is returned in "PT_term_buffer"
            PT_SPAWN(pt, &pt_input, PT_GetSerialBuffer(&pt_input));
            if (!errored_out) {

                // returns when the thread dies
                // in this case, when <enter> is pushed
                // now parse the string
                sscanf(PT_term_buffer, "%s %d %d %d", curr_day, &curr_hour, &curr_min, &curr_sec);
                set_current_day(curr_day);
                tm1.l = RtccGetTime();
                tm1.hour = (char) int2bcd(curr_hour);
                tm1.min = ( char) int2bcd(curr_min);
                tm1.sec = (char) int2bcd(curr_sec);
                RtccSetTime(tm1.l);
                medibot_bluetooth = MEDICINE_SCHEDULE;
            }
        } else if (MEDICINE_SCHEDULE == medibot_bluetooth) {
            // send the prompt via DMA to serial
            if (!errored_out) {
                sprintf(PT_send_buffer, "medicine (DAY HR MIN SEC)>\n");
            } else {
                sprintf(PT_send_buffer, "What was that?\n");
            }
            // by spawning a print thread
            PT_SPAWN(pt, &pt_DMA_output, PT_DMA_PutSerialBuffer(&pt_DMA_output));

            //spawn a thread to handle terminal input
            // the input thread waits for input
            // -- BUT does NOT block other threads
            // string is returned in "PT_term_buffer"
            PT_SPAWN(pt, &pt_input, PT_GetSerialBuffer(&pt_input));
            if (!errored_out) {

                // returns when the thread dies
                // in this case, when <enter> is pushed
                // now parse the string
                sscanf(PT_term_buffer, "%s %d %d %d", med_day, &med_hour, &med_min, &med_quantity);

                marshal_medicine();
            }
        } else if (FOLLOW == medibot_bluetooth) {
            // by spawning a print thread
            if (!errored_out) {
                // following message
                sprintf(PT_send_buffer, "Following human\n");
            } else {
                sprintf(PT_send_buffer, "What was that?\n");
            }

            // by spawning a print thread
            PT_SPAWN(pt, &pt_DMA_output, PT_DMA_PutSerialBuffer(&pt_DMA_output));

            if (human_found) {
                not_seen_iterations = 0;
                pwm_control = 35000;
                pwm_control_l = 36000;
                rc_drive_car("fwd");
//            } else if (human_found_l) {
//                not_seen_iterations = 0;
//                pwm_control = 35000;
//                rc_drive_car("left");
//            } else if (human_found_r) {
//                not_seen_iterations = 0;
//                pwm_control = 35000;
//                rc_drive_car("right");
            } else if (not_seen_iterations <= 10) {
                STOP_CAR();
                pwm_control = 0;
                pwm_control_l = 0;
                turn_time = 0;
                PT_YIELD_TIME_msec(100);
                not_seen_iterations += 1;
            } else {
                medibot_bluetooth = DRIVE;
                pwm_control = 0;
                pwm_control_l = 0;
                turn_time = 0;
                STOP_CAR();
            }
        } else if (DRIVE == medibot_bluetooth) {

            if (!errored_out) {
                sprintf(PT_send_buffer, "drive (DIR TIME PWM)>\n");
            } else {
                sprintf(PT_send_buffer, "What was that?\n");
            }

            // by spawning a print thread
            PT_SPAWN(pt, &pt_DMA_output, PT_DMA_PutSerialBuffer(&pt_DMA_output));

            //spawn a thread to handle terminal input
            // the input thread waits for input
            // -- BUT does NOT block other threads
            // string is returned in "PT_term_buffer"
            PT_SPAWN(pt, &pt_input, PT_GetSerialBuffer(&pt_input));
            if (!errored_out) {

                // returns when the thread dies
                // in this case, when <enter> is pushed
                // now parse the string
                sscanf(PT_term_buffer, "%s %d %d", rc_drive, &turn_time, &pwm_control);
                rc_drive_car(rc_drive);
                PT_YIELD_TIME_msec(turn_time);
                STOP_CAR();
            }
            turn_time = 0;

        }
    } // END WHILE(1)

    PT_END(pt);
} // thread 3

int sounded;
int sound_cycle;
// === Sound Thread =============================================
// Play desired sounds +1 score, -1 score, and game over

static PT_THREAD(protothread_sound(struct pt *pt)) {
    PT_BEGIN(pt);

    while (1) {
        PT_YIELD_TIME_msec(100);

        if (output_sound) {
            sounded++;

            for (sound_cycle = 0; sound_cycle < 250; sound_cycle++) {
                mPORTBToggleBits(BIT_14);
                PT_YIELD_TIME_msec(1);
            }

            output_sound--;
        }
        if (alarm_msg){
            sprintf(PT_send_buffer, "Medicine Time! :D\n");

            // by spawning a print thread
            PT_SPAWN(pt, &pt_DMA_output, PT_DMA_PutSerialBuffer(&pt_DMA_output));

            alarm_msg=0;
        }
        // NEVER exit while
    } // END WHILE(1)

    PT_END(pt);
}

void __ISR(_RTCC_VECTOR, ipl4) RtccIsr(void) {
    // once we get in the RTCC ISR we have to clear the RTCC int flag
    INTClearFlag(INT_RTCC);

    rtccIntCount++; // show we're progressing somehow...

    set_next_medicine();
    output_sound = 5;
    alarm_msg = 1;
}

void rtcc_init(void) {

    RtccInit();
    while (RtccGetClkStat() != RTCC_CLK_ON); // wait for SOSC - should not wait for 32ms
}

// === Main  ======================================================
// Initialize PIC32

void main(void) {
    //SYSTEMConfigPerformance(PBCLK);

    ANSELA = 0;
    ANSELB = 0;

    // === config threads ==========
    // turns OFF UART support and debugger pin, unless defines are set
    PT_setup();
    // === setup system wide interrupts  ========
    INTEnableSystemMultiVectoredInt();
    //	INTEnableInterrupts();

    //=========================================================================
    // init the threads
    PT_INIT(&pt_timer);
    PT_INIT(&pt_serial);
    PT_INIT(&pt_receive);
    PT_INIT(&pt_sound);

    // Buzzer
    mPORTBSetPinsDigitalOut(BIT_14);

    //=========================================================================
    // SETUP FOR IR SENSOR INPUT CAPTURE

    OpenTimer3(T3_ON | T3_SOURCE_INT | T3_PS_1_32, 0xffff);

    OpenCapture3(IC_EVERY_RISE_EDGE | IC_INT_1CAPTURE | IC_TIMER3_SRC | IC_ON);
    ConfigIntCapture3(IC_INT_ON | IC_INT_PRIOR_3 | IC_INT_SUB_PRIOR_3);

    INTClearFlag(INT_IC3);
    // connect PIN 17 to IC3 capture unit (middle)
    PPSInput(2, IC3, RPB8);
    mPORTBSetPinsDigitalIn(BIT_8);

    OpenCapture2(IC_EVERY_RISE_EDGE | IC_INT_1CAPTURE | IC_TIMER3_SRC | IC_ON);
    ConfigIntCapture2(IC_INT_ON | IC_INT_PRIOR_3 | IC_INT_SUB_PRIOR_3);

    INTClearFlag(INT_IC2);
    // connect PIN 18 to IC2 capture unit (left)
    PPSInput(4, IC2, RPB9);
    mPORTBSetPinsDigitalIn(BIT_9);

    OpenCapture1(IC_EVERY_RISE_EDGE | IC_INT_1CAPTURE | IC_TIMER3_SRC | IC_ON);
    ConfigIntCapture1(IC_INT_ON | IC_INT_PRIOR_3 | IC_INT_SUB_PRIOR_3);

    INTClearFlag(INT_IC1);
    // connect PIN 24 to IC1 capture unit (right)
    PPSInput(3, IC1, RPB13);
    mPORTBSetPinsDigitalIn(BIT_13);

    mPORTBSetPinsDigitalOut(BIT_3);

    pulse_mode = 0;
    counter = 0;
    // init the display
    tft_init_hw();
    tft_begin();
    tft_fillScreen(ILI9340_BLACK);

    //240x320 vertical display
    tft_setRotation(0); // Use tft_setRotation(1) for 320x240

    // seed random color
    srand(1);
    medibot_bluetooth = USER_NAME;


    // ==== MOTOR DRIVING ==============================================
    OpenTimer2(T2_ON | T2_SOURCE_INT | T2_PS_1_1, 40000);
    // LEFT WHEEL
    mPORTASetPinsDigitalOut(BIT_0);
    //PPSOutput(1, RPA0, OC1);
    mPORTBSetPinsDigitalOut(BIT_5);
    //PPSOutput(2, RPB5, OC2);

    OpenOC3(OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, 40000, 40000); //
    OpenOC4(OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, 40000, 40000);

    // RIGHT WHEEL
    //mPORTASetPinsDigitalOut(BIT_2);
    PPSOutput(3, RPA2, OC4);
    //mPORTASetPinsDigitalOut(BIT_3);
    PPSOutput(4, RPA3, OC3);

    // TURN OFF MOTORS
    STOP_CAR();

    // === RTCC  ======================================================
    rtcc_init();

    // round-robin scheduler for threads
    while (1) {
        PT_SCHEDULE(protothread_timer(&pt_timer));
        PT_SCHEDULE(protothread_serial(&pt_serial));
        PT_SCHEDULE(protothread_receive(&pt_receive));
        PT_SCHEDULE(protothread_sound(&pt_sound));
    }
} // main

// === end  ======================================================