#include <cyg/kernel/kapi.h>
#include <cyg/hal/hal_io.h>
#include "CIF.h"
#include "fft.h"
#include "timer_counter.h"
#include "pio_control.h"
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
//include <cyg/hal/hal_arch.h>
#define PS_BASE 0xFFFF4000
#define PS_PCER 0x04
#define PS_PCDR 0x04
/* now declare (and allocate space for) some kernel objects,
   like the two threads we will use */
cyg_thread thread_s[2];		/* space for two thread objects */
static cyg_interrupt intTC0;
static cyg_handle_t intTC0_handle;
static cyg_sem_t TC0_data_ready;
static cyg_sem_t TC0_transmitir;
static cyg_sem_t TC0_fin_transmision;
cyg_count32 TC0_data_ready_counter;
cyg_count32 TC0_transmitir_counter;
cyg_count32 TC0_fin_transmision_counter;
#define CYGNUM_HAL_PRI_HIGH 0
cyg_uint16 TC0_TALTO;
cyg_uint16 TC0_PERIODO;

char stack[2][4096];		/* space for two 4K stacks */
#define TC0_MAX_DATA 2048
#define TC0_NDAT 5
cyg_uint16 TC0_RAM[TC0_MAX_DATA]; /*Espacio para 2048 datos de 16bits para el canal0*/
cyg_uint16 TC0_RBM[TC0_MAX_DATA]; /*8K en total */
cyg_uint16 TC0_CUENTA1=0;
cyg_uint16 TC0_CUENTA2=0;
/* now the handles for the threads */
cyg_handle_t main_thread;
cyg_handle_t instrumentation_thread;
/* and now variables for the procedure which is the thread */
cyg_thread_entry_t main_program;
cyg_thread_entry_t instrumentation_program;

/* and now a mutex to protect calls to the C library */
cyg_mutex_t cliblock;

  cyg_vector_t intTC0_vector = TC0_IRQ;

cyg_uint32 interrupt_TC0_isr(
			   cyg_vector_t vector,
			   cyg_addrword_t data) //BASADO EN eCos book, pag 48
{
  cyg_interrupt_mask(vector);
  cyg_interrupt_acknowledge(vector);
  return(CYG_ISR_HANDLED | CYG_ISR_CALL_DSR);

}
void interrupt_TC0_dsr(
		      cyg_vector_t vector,
		      cyg_ucount32 count,
		      cyg_addrword_t data)
{
  HAL_IO_REGISTER REGISTER;
  cyg_semaphore_post(&TC0_data_ready);
  REGISTER=TC_BASE+TC_CHANNEL0+TC_RA;
  HAL_READ_UINT16(REGISTER,TC0_PERIODO);
  REGISTER=TC_BASE+TC_CHANNEL0+TC_RB;
  HAL_READ_UINT16(REGISTER,TC0_TALTO);
  cyg_interrupt_unmask(vector);
}
void cyg_user_start(void)
{
  HAL_IO_REGISTER REGISTER;
  cyg_uint32 TC_REGISTER;
  printf(STR_HEADER);
  cyg_priority_t intTC0_priority = CYGNUM_HAL_PRI_HIGH;
  printf("Iniciando la configuración del TC,Medición PWM");
  //Hay que activar el periferico
  TC_REGISTER=0x00;
  TC_REGISTER=TC_REGISTER | (PIO_PER_ENABLE<<PIO_P1);
  TC_REGISTER=TC_REGISTER | (PIO_PER_ENABLE<<PIO_P4);
  TC_REGISTER=TC_REGISTER | (PIO_PER_ENABLE<<PIO_P7);
  TC_REGISTER=TC_REGISTER ^ 0xFFFFFFFF;
  printf("\n%x\n",TC_REGISTER);
  REGISTER=PIO_BASE+PIO_PER;
  printf("\n%x\n",REGISTER);

  HAL_WRITE_UINT32(REGISTER,!TC_REGISTER);
  TC_REGISTER=0x000000000;
  TC_REGISTER=TC_REGISTER | (PIO_PDR_DISABLE<<PIO_P1);
  TC_REGISTER=TC_REGISTER | (PIO_PDR_DISABLE<<PIO_P4);
  TC_REGISTER=TC_REGISTER | (PIO_PDR_DISABLE<<PIO_P7);
  printf("\n%x\n",TC_REGISTER);
  REGISTER=PIO_BASE+PIO_PDR;
  printf("\n%x\n",REGISTER);
  HAL_WRITE_UINT32(REGISTER,TC_REGISTER);
  // Primero armo el registro
  TC_REGISTER=0;
  TC_REGISTER=TC_REGISTER | (TC_BMR_NONE<<TC_BMR_XC0);
  TC_REGISTER=TC_REGISTER | (TC_BMR_NONE<<TC_BMR_XC1);
  TC_REGISTER=TC_REGISTER | (TC_BMR_NONE<<TC_BMR_XC2);
  REGISTER=TC_BASE+TC_BMR;
  HAL_WRITE_UINT32(REGISTER,TC_REGISTER);

  // Se prepara la configuracion del canal, Chanel Control Register
  TC_REGISTER=0;
  TC_REGISTER=(TC_CCR_CLKEN_ENABLE<<TC_CLKEN);
  TC_REGISTER=TC_REGISTER | (TC_CCR_CLKDIS_NONE<<TC_CLKDIS);
  TC_REGISTER=TC_REGISTER |(TC_SWTRG_TRIGGER<<TC_SWTRG);
  printf("\n%x\n",TC_REGISTER);
  // Se configuran de igual manera los tres canales
  REGISTER=TC_BASE+TC_CCR+TC_CHANNEL0;
  HAL_WRITE_UINT32(REGISTER,TC_REGISTER);
  REGISTER=TC_BASE+TC_CCR+TC_CHANNEL1;
  HAL_WRITE_UINT32(REGISTER,TC_REGISTER);
  REGISTER=TC_BASE+TC_CCR+TC_CHANNEL2;
  HAL_WRITE_UINT32(REGISTER,TC_REGISTER);
  // Se prepara la configuración de canal
  TC_REGISTER=0;
  TC_REGISTER=TC_REGISTER | (TC_CMR_CLSKS_MCK2<<TC_CMR_CLKS);
  TC_REGISTER=TC_REGISTER | (TC_CMR_CLKI_RISING<<TC_CMR_CLKI);
  TC_REGISTER=TC_REGISTER | (TC_CMR_BURST_MODE<<TC_CMR_BURST);
  TC_REGISTER=TC_REGISTER | (TC_CMR_LDBSTOP_NONE<<TC_CMR_LDBSTOP);
  TC_REGISTER=TC_REGISTER | (TC_CMR_LDBDIS_NONE<<TC_CMR_LDBDIS);
  TC_REGISTER=TC_REGISTER | (TC_CMR_ETREDG_RISING_EDGE<<TC_CMR_ETRGEDG);
  TC_REGISTER=TC_REGISTER | (TC_CMR_ABETRG_TIOA<<TC_CMR_ABETRG);
  TC_REGISTER=TC_REGISTER | (TC_CMR_CPCTRG_NONE<<TC_CMR_CPCTRG);
  TC_REGISTER=TC_REGISTER | (TC_CMR_WAVE_CAPTURE<<TC_CMR_WAVE);
  TC_REGISTER=TC_REGISTER | (TC_CMR_LDRA_RISING_EDGE<<TC_CMR_LDRA);
  TC_REGISTER=TC_REGISTER | (TC_CMR_LDRB_RISING_EDGE<<TC_CMR_LDRB);
  printf("%x\n",TC_REGISTER);
  // Se configuran de igual manera los 3 canales
  REGISTER=TC_BASE+TC_CMR+TC_CHANNEL0; // Canal 0
  printf("%x\n",REGISTER);
  HAL_WRITE_UINT32(REGISTER,TC_REGISTER);
  REGISTER=TC_BASE+TC_CMR+TC_CHANNEL1; //Canal 1
  HAL_WRITE_UINT32(REGISTER,TC_REGISTER);
  REGISTER=TC_BASE+TC_CMR+TC_CHANNEL2; //Canal 2
  HAL_WRITE_UINT32(REGISTER,TC_REGISTER);
  // Se prepara la configuracion de la Interrupcion
  //Se abilitan en el IER
  TC_REGISTER=0;
  TC_REGISTER=TC_REGISTER | (TC_IER_ENABLE<<TC_IER_LDRAS);
  printf("%x\n",TC_REGISTER);
  // Se configuran de igual manera los 3 canales
  REGISTER=TC_BASE+TC_IER+TC_CHANNEL0; // Canal 0
  HAL_WRITE_UINT32(REGISTER,TC_REGISTER);
  REGISTER=TC_BASE+TC_IER+TC_CHANNEL1; //Canal 1
  HAL_WRITE_UINT32(REGISTER,TC_REGISTER);
  REGISTER=TC_BASE+TC_IER+TC_CHANNEL2; //Canal 2
  HAL_WRITE_UINT32(REGISTER,TC_REGISTER);
  //Se confirma en el IDR
  TC_REGISTER=TC_REGISTER ^ 0xFFFFFFFF;
  printf("%x\n",TC_REGISTER);
  // Se configuran de igual manera los 3 canales
  REGISTER=TC_BASE+TC_IDR+TC_CHANNEL0; // Canal 0
  HAL_WRITE_UINT32(REGISTER,TC_REGISTER);
  REGISTER=TC_BASE+TC_IDR+TC_CHANNEL1; //Canal 1
  HAL_WRITE_UINT32(REGISTER,TC_REGISTER);
  REGISTER=TC_BASE+TC_IDR+TC_CHANNEL2; //Canal 2
  HAL_WRITE_UINT32(REGISTER,TC_REGISTER);
  //Una ves configurados se activan en el PS
  TC_REGISTER=0xFFFFFFFF;
  REGISTER=PS_BASE+PS_PCER;
  HAL_WRITE_UINT32(REGISTER,TC_REGISTER);
  TC_REGISTER=0x00;
  REGISTER=PS_BASE+PS_PCDR;
  HAL_WRITE_UINT32(REGISTER,TC_REGISTER);
  // Preparando el sistem para antender las interrupciones
  cyg_interrupt_create(
		       intTC0_vector,
		       intTC0_priority,
		       0,
		       &interrupt_TC0_isr,
		       &interrupt_TC0_dsr,
		       &intTC0_handle,
		       &intTC0);
  cyg_interrupt_attach(intTC0_handle);
  cyg_interrupt_unmask(intTC0_vector);

  cyg_semaphore_init(&TC0_data_ready,TC0_data_ready_counter);
  cyg_semaphore_init(&TC0_transmitir,TC0_transmitir_counter);
  cyg_semaphore_init(&TC0_fin_transmision,TC0_fin_transmision_counter);

  cyg_mutex_init(&cliblock);

  cyg_thread_create(6, main_program, (cyg_addrword_t) 0,
		    "MAIN THREAD", (void *) stack[0], 4096,
		    &main_thread, &thread_s[0]);
  cyg_thread_create(3, instrumentation_program, (cyg_addrword_t) 0,
		    "Instrumentation THREAD", (void *) stack[1], 4096,
		    &instrumentation_thread, &thread_s[1]);
  cyg_thread_resume(main_thread);
  cyg_thread_resume(instrumentation_thread);
}

/* this is a simple program which runs in a thread */
void main_program(cyg_addrword_t data)
{
  //  int message = (int) data;
  // int delay;
  int datos_cuenta;
  printf("\nIniciando mediciones,esperando %i datos\n",TC0_MAX_DATA);

  for (;;) {
    cyg_semaphore_wait(&TC0_transmitir);
    for(datos_cuenta=0;datos_cuenta<TC0_MAX_DATA;datos_cuenta++){
      printf("%i\t%i",TC0_RAM[datos_cuenta],TC0_RBM[datos_cuenta]);
    }
    cyg_semaphore_post(&TC0_fin_transmision);
  }
}
void instrumentation_program(cyg_addrword_t data)
{
  while(1){
    cyg_semaphore_wait(&TC0_data_ready); // Espera a que exista un dato proveniente del PWM
    printf("1");
    if ((TC0_CUENTA1++)==TC0_NDAT){ // Espera que hayan pasado TC0_NDAT datos
      TC0_CUENTA1=0; // Renicia la cuenta de datos
      TC0_RAM[TC0_CUENTA2]=TC0_TALTO; //Guarda los datos correspondientes en los arreglos
      TC0_RBM[TC0_CUENTA2]=TC0_PERIODO;
      if(TC0_CUENTA2==TC0_MAX_DATA) // Si se ha llenado el arreglo de datos, se activa el semaforo de TC0_transmitir y espera que se haya terminado la transmision a traves del semaforo TC0_fin_transmision, para luego reiniciar la cuenta, desactiva la interrupcion y la activa al finalizar la transmision
	{
	  cyg_interrupt_mask(intTC0_vector);
	  cyg_semaphore_post(&TC0_transmitir);
	  TC0_CUENTA2=0;
	  cyg_semaphore_wait(&TC0_fin_transmision);
	  cyg_interrupt_unmask(intTC0_vector);
	}
      else{
	
	    TC0_CUENTA2++; //Incrementa el contador del arreglo
	  }
    }
  }

}