#include "main.h"

#define REV "$Rev$"

unsigned int32 time;   // pocitadlo preteceni casovace

#define BUFLEN 100

#int_RTCC
void  RTCC_isr(void)    // preruseni od pretekleho casovace 
{
  time++;
}

// Includes all USB code and interrupts, as well as the CDC API
#include <usb_cdc.h>
#include <math.h>

float quadraticerror(float average, float buf[], int16 size) // compute average quadratic error
{
  int16 i;
  float err=0;

   for(i=0; i<size; i++) err += (buf[i]-average)*(buf[i]-average);      // sum quadratic errors   
   err = sqrt((1/(float)size)*err);
   return err;
}

void main()
{

float x[BUFLEN], y[BUFLEN], z[BUFLEN];
float xavg, yavg, zavg;
int i;
   port_b_pullups(FALSE);
   setup_psp(PSP_DISABLED);
   setup_spi(SPI_SS_DISABLED);
   setup_wdt(WDT_OFF);
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_256);
   setup_timer_1(T1_DISABLED);
   setup_timer_2(T2_DISABLED,0,1);
   setup_timer_3(T3_DISABLED|T3_DIV_BY_1);
   setup_comparator(NC_NC_NC_NC);
   setup_vref(FALSE);
   enable_interrupts(INT_TIMER0);
   enable_interrupts(GLOBAL);
   
   setup_adc_ports(AN0_TO_AN2|VSS_VREF);
   setup_adc(ADC_CLOCK_DIV_32);

   usb_init(); // initialise USB module
   
   while(!usb_cdc_connected());
   time=0;
   set_timer0(0);
   printf(usb_cdc_putc,"time[s]  X  Xerr  Y  Yerr  Z  Zerr \n\r");

   while(usb_cdc_connected())  // pockej nez se pripoji seriovy port PC
   {
      for(i=0; i <BUFLEN; i++)
      {
         set_adc_channel(0);
         delay_us(10);
         x[i]=read_adc();
         xavg+=x[i];
         
         set_adc_channel(1);
         delay_us(10);
         y[i]=read_adc();
         yavg+=y[i];

         set_adc_channel(2);
         delay_us(10);
         z[i]=read_adc();
         zavg+=z[i];
      }
      
      xavg=xavg/BUFLEN;
      yavg=yavg/BUFLEN;
      zavg=zavg/BUFLEN;

      // odesli namerene hodnoty
      printf(usb_cdc_putc, "%7.3f %4.3f %4.3f %4.3f %4.3f %4.3f %4.3f \n\r",((time << 16) + get_timer0())/15625.0, xavg, quadraticerror(xavg,x,BUFLEN), yavg, quadraticerror(yavg,y,BUFLEN), zavg, quadraticerror(zavg,z,BUFLEN)); //konstanta k je kvuli prevodu do rozzumnych jednotek [s]
   }
}