/***********************************************************************************/
/* Created By Troy Davis and Caitlin Smart                                         */
/* Built upon ECE492 lab code as well as altera hello world ucii demo              */
/***********************************************************************************/

//#include <errno.h>
//#include <ctype.h>
//#include <unistd.h>
#include <stdio.h>
//#include <stdlib.h>
//#include <string.h>
//#include "includes.h"
#include "i2c/i2c_ctrl.h"
#include "altera_avalon_timer_regs.h"
#include "priv/alt_file.h"
#include "accelerometer/accelerometer.h"
//#include "sys/alt_irq.h"
#include "sys/alt_flash.h"
#include "sys/alt_flash_dev.h"
#include "system.h"
#include "alt_types.h"


/* Definition of Task Stacks */
#define   TASK_STACKSIZE       2048
#define   NUM_ELEMENTS         12
#define   WAIT_FOREVER         0
#define   HIST_LENGTH          50
/* Definition of Task Priorities */
#define TASK_ACCELEROMETER_PRIORITY      1


OS_STK    task_accelerometer_stk[TASK_STACKSIZE];

OS_EVENT *LCD_sem;

// ****************************GLOBALS********************************** //

FILE* fp; // lcd

// ******************************************************************** //

/***********************************************************************************/
/*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)
{
    char error;   
    int interrupts;
    int count = 0;
    int i;
    int spm_history[HIST_LENGTH];
    float spm_average = 0;

    // initialize i2c master and accelerometer controller
    init_i2c();
    init_accelerometer();
        
    for(i = 0; i <= HIST_LENGTH-1; i++){ // Calc current average from history 
        spm_history[i] = 1;
        }
    
    while (1)
    { 
        
        interrupts = (int)IORD_32DIRECT(ACCELEROMETER_CONTROLLER_BASE,0);// read data from accel controller
        printf("interrupts: %d\n",interrupts);
        IOWR_32DIRECT(ACCELEROMETER_CONTROLLER_BASE,0,0); // Clear interrupt count reg in accel controller
        
        if(count == HIST_LENGTH)
            count = 0; // reset count
            
        for(i = 0; i <= HIST_LENGTH-1; i++){ // Calc current average from history 
            if(i == 0){
                spm_average = spm_history[i];
            }
            else{
                spm_average = spm_average + spm_history[i];
            }
        }
        
        /*  
            Do this after calculating current average from histoy because we get
            a extra sample this way. ie we can average the past 50 plus the one we
            just got rather than overwrite one of the histories then average 50 
            samples
        */
        spm_history[count] = interrupts; // update history
        
        spm_average = (spm_average + spm_history[count])/51; // update average with current sample
        spm_average = spm_average * 120;

        OSSemPend(LCD_sem, WAIT_FOREVER, &error);// Protect LCD writes
            fprintf(fp, "\x1B[1;1H");// Move to top left (x,y) = (1,1)
            fprintf(fp, "\x1B[K");// Clear top line
            fprintf(fp, "SPM: %f \n",spm_average);
        OSSemPost(LCD_sem);// Release LCD writes
        
        for(i = 0; i <= HIST_LENGTH-1; i++){ // Calc current average from history and display on stdout
            if(i == 0){
            printf("{ %d",spm_history[i]);
            }
            else{
            printf(" %d",spm_history[i]);
            }           
        }
        
        printf("}\n");       
        printf("count: %d\n",count);
        OSTimeDlyHMSM(0,0,0,500);
        count++;          
  } 
}

/***********************************************************************************/
/* END ACCELEROMETER TASK                                                          */
/***********************************************************************************/

/*********************************************************************************/
/* The main function creates two task, initializes queue and starts multi-tasking*/
/*********************************************************************************/

int main(void)
{ 
  
    OSInit();

    //Open a file write pipe to LCD
    fp = fopen ("/dev/lcd_0", "w");
    if(fp == NULL){
        printf("Failed to open LCD\n");
    }

    // Initalize Semaphores
    LCD_sem = OSSemCreate(1);
    
    // CREATE TASKS                
  OSTaskCreateExt(task_accelerometer,
                  NULL,
                  (void *)&task_accelerometer_stk[TASK_STACKSIZE-1],
                  TASK_ACCELEROMETER_PRIORITY,
                  TASK_ACCELEROMETER_PRIORITY,
                  task_accelerometer_stk,
                  TASK_STACKSIZE,
                  NULL,
                  0);

  
  // BEGIN MULTI TASKING
  OSStart();
  
  return 0;
}

/************************************************************************/
