/**
 * TinyFeather Autopilot Program
 * 
 */
#include <std.h>

#include "config.h"

// 標準C/C++ヘッダ
#include <cstdio>
#include <cmath>
#include <xutility>
#include <new>

// DSP BIOS ヘッダ
#include <log.h>
#include <idl.h>
#include <sem.h>
#include <tsk.h>
#include <clk.h>
#include <gbl.h>

#include <ti/pspiom/cslr/soc_C6747.h>
#include <ti/pspiom/cslr/cslr_pllc_C6747.h>

#include "kernel.h"

using namespace std;

extern "C" {
// Missing functions
double cbrt(double x){return pow(x, (1.0 / 3));}
double erf(double x){
  // @see http://www.johndcook.com/cpp_erf.html
  // constants
  static const double a1(0.254829592);
  static const double a2(-0.284496736);
  static const double a3( 1.421413741);
  static const double a4(-1.453152027);
  static const double a5(1.061405429);
  static const double p(0.3275911);

  // Save the sign of x
  int sign = 1;
  if (x < 0){sign = -1;}
  x = fabs(x);

  // A&S formula 7.1.26
  double t(1.0 / (1.0 + p * x));
  double y(1.0 - (((((a5 * t + a4) * t) + a3) * t + a2) * t + a1) * t * exp(-x*x));

  return sign * y;
}
double erfc(double x){return 1.0 - erf(x);}
int signbit(double x){return (x == -0e0) || (x < 0e0);}
#if __TI_COMPILER_VERSION__ < 7000000
double hypot(double x, double y){
  return sqrt((x * x) + (y * y));
}
#endif
void tzset(){}
}

void check_tsk(){
	TSK_Stat stat;
  /*TSK_stat(&tsk_RT, &stat);
  LOG_printf(&trace, "RT_0: %d, %d", stat.attrs.priority, stat.mode);
  LOG_printf(&trace, "RT_1: %d, %d", stat.used, stat.attrs.stacksize);
  TSK_stat(&tsk_MU, &stat);
  LOG_printf(&trace, "MU_0: %d, %d", stat.attrs.priority, stat.mode);
  LOG_printf(&trace, "MU_1: %d, %d", stat.used, stat.attrs.stacksize);*/
}

void canary_bird() {
  static int invoked(0);
  invoked++;
  MEM_Stat statbuf;
  MEM_stat(MEM->MALLOCSEG, &statbuf);
  if(statbuf.size <= statbuf.used){
    SYS_exit(-1);
  }
}

void my_new_handler() {
  MEM_Stat statbuf;
  MEM_stat(MEM->MALLOCSEG, &statbuf);
  static int invoked(0);
  invoked++;
  LOG_printf(&trace, "seg %d: O 0x%x", MEM->MALLOCSEG, statbuf.size);
  LOG_printf(&trace, "\tU 0x%x\tA 0x%x", statbuf.used, statbuf.length);
  SYS_exit(-1);
}

int main(){
  std::set_new_handler(my_new_handler);
  
  { // check clock configuration
    CSL_PllcRegsOvly pllcRegs((CSL_PllcRegsOvly)CSL_PLLC_0_REGS);
    Uint32 pllm(pllcRegs->PLLM & 0x1F), prediv(pllcRegs->PREDIV & 0x1F), postdiv(pllcRegs->POSTDIV & 0x1F);
    Uint32 core_freq_KHz(GBL_getClkIn() * (pllm + 1) / (prediv + 1) / (postdiv + 1));
    GBL_setFrequency(core_freq_KHz);
    CLK_reconfig();

    LOG_printf(&trace, "Core is %d KHz.", core_freq_KHz);
  }

  LOG_printf(&trace, "TinyFeather started.");
  
  Kernel::get_instance().initialize();
  
  return 0;
}