
#include "basic_io.h"
//#include "LCD.h"
#include "SD_Card.h"
#include "altera_avalon_pio_regs.h"
#include "includes.h"
#include <stdio.h>
#include <stdbool.h>
#include "accelerometer.h"
#include "i2c_ctrl.h"

////#define ACCEL_BASE    0x1D  //SA0 breakout board default to 1
////#define INT_SOURCE    0x0C
////#define WHO_AM_I      0x0D
////#define CTRL_REG4     0x2D
////#define CTRL_REG5     0x2E
////#define FF_MT_CFG     0x15
////#define FF_MT_SRC     0x16
////#define FF_MT_THS     0x17
////#define FF_MT_COUNT   0x18
////
//int main(void)
//{
////    unsigned char test;
////    while(1){
////    //init_accelerometer();
////    //accelerometer_read(0x0D);
////    test=i2c_read(ACCEL_BASE,WHO_AM_I);
////    printf("test is: %x\n",test);
////    }
//  
//    init_i2c();
//  init_accelerometer();
// // i2c_write (0x34, 0x047B);
//    
////  UINT16 i=0,Tmp1=0,Tmp2=0;
////  UINT32 j=720;
////  BYTE Buffer[512]={0};
////
////  SD_card_init();
////  //LCD_Test();
////  while(1)
////  {
////    //printf("%d\n",IORD(ACCEL_SDA_BASE,0));
////    SD_read_lba(Buffer,j,1);
////    while(i<512)
////    {
////      if(!IORD(AUDIO_0_BASE,0))
////      {
////        Tmp1=(Buffer[i+1]<<8)|Buffer[i];
////        IOWR(AUDIO_0_BASE,0,Tmp1);
//////          int test = i2c_read (0x34, 0x04);
//////            printf("%x\n",test);
////        i+=2;
////      }
////    }
////    IOWR(SEG7_DISPLAY_BASE,0,j);
////    j++;
////    i=0;
////  }
//// 
////  return 0;
//}

//-------------------------------------------------------------------------


//* Definition of Task Stacks */
#define   TASK_STACKSIZE       2048
OS_STK    task_which_note_stk[TASK_STACKSIZE];
OS_STK    task_sd_play_stk[TASK_STACKSIZE];
OS_STK    task_ms_stk[TASK_STACKSIZE];
//////////////////////////////////////////////////
OS_STK     task_accelerometer_stk[TASK_STACKSIZE];
///////////////////////////////////////////////////

/* Definition of Task Priorities */
#define TASK_WHICH_NOTE_PRIORITY    1
#define TASK_SD_PLAY_PRIORITY       4
#define TASK_MS_PRIORITY            3
///////////////////////////////////////////////
#define TASK_ACCELEROMETER_PRIORITY      2
/////////////////////////////////////////////////

//Eric's add-on.
OS_EVENT *MSQ;
OS_EVENT *SQ;
// globals! AHH
UINT32 j=0;
//UINT32 range_end=7900;
UINT32 range_end=0;
int tg = 0;
int ms = 0;//default 0 is note mode. 1 is chord mode.
int rst = 0;
int oct = 0;
/* Figures out which tap sensor is engaged and passes it to task_sd_play via a global */
void task_which_note(void* pdata)
{
   // printf("debug in task which note**********************************\n");
    void *SM[10];
    SQ = OSQCreate(&SM[0], 10);
    INT8U err;
    void *Smsg;
    int swcnt = 0;
    while(1){
            Smsg = OSQPend(SQ, 0, &err);
            if(err == OS_NO_ERR){
                tg = 1;
                //printf("Before Reading from smsg;\n");
                INT8U switches = IORD_ALTERA_AVALON_PIO_DATA( Smsg );
                switches = switches % 254;
                swcnt = swcnt + switches;
                //printf("%d\n",swcnt);
                if (rst%2 == 1){ //reset
                    rst = 0;
                    //printf("RST\n");
                }
                if(ms%2 == 0){
                    //Default Notes
                    //Default: notes
                   // printf("Note Mode\n");
                    if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEA_BASE)){   //tap sensors are active high
                        //printf("Play note A\n");
                        j=8150;
                        range_end=9050; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEB_BASE)){  
                        //printf("Play note B\n");
                        j=9200;
                        range_end=9800; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEC_BASE)){  
                        //printf("Play note C\n");
                        j=9850;
                        range_end=10600; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTED_BASE)){  
                        //printf("Play note D\n");
                        j=10650;
                        range_end=11400; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEE_BASE)){  
                        //printf("Play note E\n");
                        j=11500;
                        range_end=12100; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEF_BASE)){  
                        //printf("Play note F\n");
                        j=12200;
                        range_end=12900; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEG_BASE)){  
                        //printf("Play note G\n");
                        j=13000;
                        range_end=13800; } 
                    else {
                        tg = 0;
                    }
                }
                if(ms%2 == 1){
                    //Default Chord mode
                   // printf("Chord Mode\n");
                    if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEA_BASE)){   //tap sensors are active high
                       // printf("Play chord A\n");
                        j=26900;
                        range_end=27600; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEB_BASE)){  
                        //printf("Play chord B\n");
                        j=27700;
                        range_end=28600; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEC_BASE)){  
                        //printf("Play chord C\n");
                        j=28700;
                        range_end=29600; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTED_BASE)){  
                        //printf("Play chord D\n");
                        j=29700;
                        range_end=30600; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEE_BASE)){  
                        //printf("Play chord E\n");
                        j=30700;
                        range_end=31600; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEF_BASE)){  
                        //printf("Play chord F\n");
                        j=31650;
                        range_end=32500; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEG_BASE)){  
                        //printf("Play chord G\n");
                        j=32500;
                        range_end=33300; } 
                    else {
                        tg = 0;
                    }
                }
                if(ms%2 == 0 && oct == 1){
                    //printf("NOTE AND OCTUP.\n");
                    
                    if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEA_BASE)){   //tap sensors are active high
                        //printf("Play note A\n");
                        j=14000;
                        range_end=14900; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEB_BASE)){  
                        //printf("Play note B\n");
                        j=15000;
                        range_end=15600; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEC_BASE)){  
                        //printf("Play note C\n");
                        j=15700;
                        range_end=16300; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTED_BASE)){  
                        //printf("Play note D\n");
                        j=16400;
                        range_end=17100; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEE_BASE)){  
                        //printf("Play note E\n");
                        j=17250;
                        range_end=17900; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEF_BASE)){  
                        //printf("Play note F\n");
                        j=18000;
                        range_end=18700; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEG_BASE)){  
                        //printf("Play note G\n");
                        j=18800;
                        range_end=19500; } 
                    else {
                        tg = 0;
                    }
                }
                if(ms%2 == 0 && oct == -1){
                    //printf("NOTE AND OCTDOWN.\n");
                    //Default: notes
                    //printf("Note Mode\n");
                    if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEA_BASE)){   //tap sensors are active high
                        //printf("Play note A\n");
                        j=1800;
                        range_end=2500; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEB_BASE)){  
                        //printf("Play note B\n");
                        j=2900;
                        range_end=3400; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEC_BASE)){  
                        //printf("Play note C\n");
                        j=3600;
                        range_end=4200; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTED_BASE)){  
                        //printf("Play note D\n");
                        j=4500;
                        range_end=5200; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEE_BASE)){  
                        //printf("Play note E\n");
                        j=5400;
                        range_end=5900; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEF_BASE)){  
                        //printf("Play note F\n");
                        j=6250;
                        range_end=6800; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEG_BASE)){  
                        //printf("Play note G\n");
                        j=7100;
                        range_end=7900; } 
                    else {
                        tg = 0;
                    }
                }
                if(ms%2 == 1 && oct == 1){
                    //printf("CHORD AND OCTUP.\n");
                   // printf("Note Mode\n");
                    if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEA_BASE)){   //tap sensors are active high
                        //printf("Play note A\n");
                        j=33600;
                        range_end=34400; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEB_BASE)){  
                        //printf("Play note B\n");
                        j=34500;
                        range_end=35350; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEC_BASE)){  
                        //printf("Play note C\n");
                        j=35450;
                        range_end=36250; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTED_BASE)){  
                        //printf("Play note D\n");
                        j=36300;
                        range_end=37100; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEE_BASE)){  
                        //printf("Play note E\n");
                        j=37200;
                        range_end=38000; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEF_BASE)){  
                        //printf("Play note F\n");
                        j=38100;
                        range_end=38900; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEG_BASE)){  
                        //printf("Play note G\n");
                        j=39050;
                        range_end=39800; } 
                    else {
                        tg = 0;
                    }
                }
                if(ms%2 == 1 && oct == -1){
                    //printf("CHORD AND OCTDOWN.\n");
                    //printf("Note Mode\n");
                    if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEA_BASE)){   //tap sensors are active high
                        //printf("Play note A\n");
                        j=19700;
                        range_end=20400; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEB_BASE)){  
                        //printf("Play note B\n");
                        j=20800;
                        range_end=21400; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEC_BASE)){  
                        //printf("Play note C\n");
                        j=21800;
                        range_end=22450; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTED_BASE)){  
                        //printf("Play note D\n");
                        j=22800;
                        range_end=23500; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEE_BASE)){  
                        //printf("Play note E\n");
                        j=23800;
                        range_end=24600; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEF_BASE)){  
                        //printf("Play note F\n");
                        j=24800;
                        range_end=25600; }
                    else if(!IORD_ALTERA_AVALON_PIO_DATA(NOTEG_BASE)){  
                        //printf("Play note G\n");
                        j=25800;
                        range_end=26600; } 
                    else {
                        tg = 0;
                    }
                }
        }
        else{
            //err do nothing.
        }
                
    } //while(1)
}
//isr for dist sensor
static void isr_DS (){
   //OSQPost(SQ, IORD_ALTERA_AVALON_PIO_EDGE_CAP(DIST_SENSOR_BASE));
   IOWR_ALTERA_AVALON_PIO_EDGE_CAP(DIST_SENSOR_BASE,1);

}
//isr for MS
static void isr_ModeSwitch (){
   ms = ms + 1;
   OSQPost(MSQ, IORD_ALTERA_AVALON_PIO_EDGE_CAP(MS_BASE));
   IOWR_ALTERA_AVALON_PIO_EDGE_CAP(MS_BASE,1);
}
//isr for RST
static void isr_Rst(){
   rst = rst + 1;
   ms = 0;
   oct = 0;
   OSQPost(MSQ, IORD_ALTERA_AVALON_PIO_EDGE_CAP(RST_BASE));
   IOWR_ALTERA_AVALON_PIO_EDGE_CAP(RST_BASE,1);
}
//isr for octup
static void isr_Octup (){
   oct = oct + 1;
   if(oct > 1){
    oct = 1;
   }
   OSQPost(MSQ, IORD_ALTERA_AVALON_PIO_EDGE_CAP(OCT_UP_BASE));
   IOWR_ALTERA_AVALON_PIO_EDGE_CAP(OCT_UP_BASE,1);
}
//isr for octdown
static void isr_Octdown(){
   oct = oct - 1;
   if(oct < -1){
    oct = -1;
   }
   OSQPost(MSQ, IORD_ALTERA_AVALON_PIO_EDGE_CAP(OCT_DOWN_BASE));
   IOWR_ALTERA_AVALON_PIO_EDGE_CAP(OCT_DOWN_BASE,1);
}
/* Plays .wav from SD card with switch up and glove button down */
void task_sd_play(void* pdata)
{
   // printf("debug in task sd play**********************************\n");
    UINT32 range_start=0;
    BYTE Buffer[512]={0};
    UINT16 i=0,Tmp1=0;
    //printf("Please insert SD card.\n");  
    while(SD_card_init())    //will return 1 and continuously loop until SD card inserted
        usleep(500000);    
    LCD_Test();
    //printf("SD card detected.\n");
    
//playing note

    while(1) {
        SD_read_lba(Buffer,j,1);  
        range_start=j;  //won't work, j++ rem? how to get it so it will play same note every switch flick?      
        if (tg == 1) {
               
            if(j<range_end) {   //restrict to only play 1 note/section
                while(i<512) { 
                    if(!IORD(AUDIO_0_BASE,0)) {  
                        Tmp1=(Buffer[i+1]<<8)|Buffer[i];
                        IOWR(AUDIO_0_BASE,0,Tmp1);
                        i+=2;
                    } //if(!IORD(AUDIO_0_BASE,0))
                } //while(i<512)
                IOWR(SEG7_DISPLAY_BASE,0,j);
                j++;
                i=0;
            } //if(j<range_end) 
        } //if play on switch
        else
            j=range_start; //reset j
    } //while(1)

} //end task_sd_play

void TaskModeDisplay(void* pdata)
{
    //printf("debug in task mode display**********************************\n");
    char* nm0 = "NOTE MODE|OCT.0";
    char* cm0 = "CHORD MODE|OCT.0";
    char* nm1 = "NOTE MODE|OCT.+";
    char* cm1 = "CHORD MODE|OCT.+";
    char* nm_1 = "NOTE MODE|OCT.-";
    char* cm_1 = "CHORD MODE|OCT.-";
    char* clr = "\x1B[\x32\x4A";
    INT8U err;
    void *Qmsg;
    void *MS[10];
    MSQ = OSQCreate(&MS[0], 10);
    FILE *fp;
    fp = fopen ("/dev/lcd_16207_0","w");
    fprintf(fp, "%s", clr);
    fprintf(fp, "%s", nm0);
    
    while(1){
        //printf("mode swtich task\n");
        Qmsg = OSQPend (MSQ, 0, &err);
        //printf("after ms pend\n");
        if(err == OS_NO_ERR){
            if (ms%2 == 1){//chord mode
                if(oct == 0){
                    fprintf(fp, "%s", clr);
                    fprintf(fp, "%s", cm0);
                }
                else if(oct == 1 || oct >1){
                    fprintf(fp, "%s", clr);
                    fprintf(fp, "%s", cm1);
                }
                else {
                    fprintf(fp, "%s", clr);
                    fprintf(fp, "%s", cm_1);
                }
            }
            if (ms%2 == 0){//note mode
                if(oct == 0){
                    fprintf(fp, "%s", clr);
                    fprintf(fp, "%s", nm0);
                }
                else if(oct == 1 || oct >1){
                    fprintf(fp, "%s", clr);
                    fprintf(fp, "%s", nm1);
                }
                else {
                    fprintf(fp, "%s", clr);
                    fprintf(fp, "%s", nm_1);
                }
            }
        }
    }
}



//////////////////////////////////
void task_accelerometer(void* pdata)
{
    void *msg;
    INT8U err;
    int freq;
    int message;
    int a;
    init_i2c();
    init_accelerometer();
  while (1)
  { 
   message = (int)IORD_32DIRECT(ACCEL_CONTROL_BASE,0);
   int rate = message / 1;
   //printf(" pace %d \n",rate);
   IOWR_32DIRECT(ACCEL_CONTROL_BASE,0,0);
   //OSTimeDlyHMSM(0,0,1,0);   
   
   if(rate > 0){
    OSQPost(SQ, IORD_ALTERA_AVALON_PIO_EDGE_CAP(DIST_SENSOR_BASE));
   }
   OSTimeDlyHMSM(0,0,0,100);
  }
  
}
///////////////////////////////////

// The main function creates task_sd_play 
int main(void)
{
    printf("Hello world!\n"); 
    
    /////////////////////////////////////////////////////
    
   /////////////////////////////////////////////////////////////////////// 
    
       
    //isr_DS
    alt_irq_register (DIST_SENSOR_IRQ, NULL, isr_DS);
    IOWR_ALTERA_AVALON_PIO_IRQ_MASK(DIST_SENSOR_BASE, 1);
    //isr_MS
    alt_irq_register (MS_IRQ, NULL, isr_ModeSwitch);
    IOWR_ALTERA_AVALON_PIO_IRQ_MASK(MS_BASE, 1);
    //isr_RST
    alt_irq_register (RST_IRQ, NULL, isr_Rst);
    IOWR_ALTERA_AVALON_PIO_IRQ_MASK(RST_BASE, 1);
    //isr_OU
    alt_irq_register (OCT_UP_IRQ, NULL, isr_Octup);
    IOWR_ALTERA_AVALON_PIO_IRQ_MASK(OCT_UP_BASE, 1);
    //isr_OD
    alt_irq_register (OCT_DOWN_IRQ, NULL, isr_Octdown);
    IOWR_ALTERA_AVALON_PIO_IRQ_MASK(OCT_DOWN_BASE, 1);
   OSInit();
   OSTaskCreateExt(task_which_note,
                  NULL,
                  (void *)&task_which_note_stk[TASK_STACKSIZE-1],
                  TASK_WHICH_NOTE_PRIORITY,
                  TASK_WHICH_NOTE_PRIORITY,
                  task_which_note_stk,
                  TASK_STACKSIZE,
                  NULL,
                  0);
                  
  OSTaskCreateExt(task_sd_play,
                  NULL,
                  (void *)&task_sd_play_stk[TASK_STACKSIZE-1],
                  TASK_SD_PLAY_PRIORITY,
                  TASK_SD_PLAY_PRIORITY,
                  task_sd_play_stk,
                  TASK_STACKSIZE,
                  NULL,
                  0);
  OSTaskCreateExt(TaskModeDisplay,
                  NULL,
                  (void *)&task_ms_stk[TASK_STACKSIZE-1],
                  TASK_MS_PRIORITY,
                  TASK_MS_PRIORITY,
                  task_ms_stk,
                  TASK_STACKSIZE,
                  NULL,
                  0);
  OSTaskCreateExt(task_accelerometer,
                  NULL,
                  (void *)&task_accelerometer_stk[TASK_STACKSIZE-1],
                  TASK_ACCELEROMETER_PRIORITY,
                  TASK_ACCELEROMETER_PRIORITY,
                  task_accelerometer_stk,
                  TASK_STACKSIZE,
                  NULL,
                  0);

  OSStart();
  return 0;
}

///***********************************************************************************/
///* Created By Troy Davis and Caitlin Smart                                         */
///* Built upon ECE492 lab code as well as altera hello world ucii demo              */
///***********************************************************************************/
//
//#include <stdio.h>
//#include <stdlib.h>
//#include "includes.h"
////#include "Audio/audio_codec_WM8731.h"
////#include "altera_up_avalon_audio.h"
//#include "i2c_ctrl.h"
////#include "SD_CARD/sd_controller.h"
////#include "player_lib.h"
//#include "altera_avalon_timer_regs.h"
//#include "priv/alt_file.h"
//#include "accelerometer.h"
//#include "sys/alt_irq.h"
//#include "sys/alt_flash.h"
//#include "sys/alt_flash_dev.h"
//#include "system.h"
//
//
///* Definition of Task Stacks */
//#define   TASK_STACKSIZE       2048
//#define   NUM_ELEMENTS         12
//#define   WAIT_FOREVER         0
//
//OS_STK    task_accelerometer_stk[TASK_STACKSIZE];
//OS_STK    task_mp3player_stk[TASK_STACKSIZE];
//
//OS_EVENT *msgQueue;
//// This is our audio output device
////alt_audio_codec* audio_codec_device;
//// This is our audio output device in test
////alt_up_audio_dev * audio_codec_device;
////This is our SD card controller
////sd_card_info_struct* sd_card_controller;
//
///* Definition of Task Priorities */
//#define TASK_ACCELEROMETER_PRIORITY      1
//#define TASK_MP3PLAYER_PRIORITY          2
//
////FUNCTION DEF
//void AA_Play_Wav();
//
///***********************************************************************************/
///*ACCELEROMETER TASK:                                                              */
///*Monitors the interrupt count register generated by the accelerometer and updates */
///* a queue every five seconds with the pace of the jogger.                         */
///***********************************************************************************/
//void task_accelerometer(void* pdata)
//{
//    void *msg;
//    INT8U err;
//    int freq;
//    int message;
//    int a;
//    init_i2c();
//    init_accelerometer();
//  while (1)
//  { 
//    /*msg = OSQPend(msgQueue,WAIT_FOREVER, &err);
//            if((int)msg == 1 ){
//                freq = (int) OSQPend(msgQueue,WAIT_FOREVER, &err);
//                printf("Pace: %d\n", freq);
//            }*/
//            
//   message = (int)IORD_32DIRECT(ACCEL_CONTROL_BASE,0);
//   int rate = message / 1;
//   if((a=(int)accelerometer_read(FF_MT_SRC))==0x10)
//   printf(" %d \n",rate);
//   else{
//    printf(" ========== %d \n",rate);
//    printf("--------------0x%x\n", a);
//   }
//   IOWR_32DIRECT(ACCEL_CONTROL_BASE,0,0);
//   OSTimeDlyHMSM(0,0,1,0);   
//  }
//  
//}
//
///***********************************************************************************/
///* END ACCELEROMETER TASK                                                          */
///***********************************************************************************/
//
///*********************************************************************************/
///* The main function creates two task, initializes queue and starts multi-tasking*/
///*********************************************************************************/
//
//int main(void)
//{ 
//  
//  OSInit();
//  
//  // Create Queue
//  void * msgQueueStorage[NUM_ELEMENTS];
//  msgQueue = OSQCreate(msgQueueStorage,NUM_ELEMENTS);
//  if(msgQueue == NULL)
//  {
//    printf("error in Queue create\n");
//  }
//  
//  OSTaskCreateExt(task_accelerometer,
//                  NULL,
//                  (void *)&task_accelerometer_stk[TASK_STACKSIZE-1],
//                  TASK_ACCELEROMETER_PRIORITY,
//                  TASK_ACCELEROMETER_PRIORITY,
//                  task_accelerometer_stk,
//                  TASK_STACKSIZE,
//                  NULL,
//                  0);
//  
//  
//  OSStart();
//  
//  return 0;
//}
//
///***************************************************************************/
//
////-----------------------------------------------------------------------------------
