#include <rtl_conf.h>
#include <deblin/vhal.h>
#include <time.h>
#include <pthread.h>
#include <rtl_ipc.h>
#include <semaphore.h>
#include <rtl_barrier.h>
#include <rtl_printf.h>
#include <unistd.h>
#include "examples.h"
//#include <math.h>

#if TANK_EXAMPLE_NO_FB 
#include <asm/msr.h>

#if CONFIG_RTL_GUI
#include <PTK.h>
#include "logo.c"
#endif

 // Variable usada en la visualización del nivel
static double nivel_max = 50;
static double temp_max = 50;
static int nivel_current = 20;

 // Variables para la simulación
static double Nivel= 25.00;
static double Temp = 15.00;
static double Qt = 16.00;
static double Qn = 0;
static double Qs = 30.00;
static double Qtotal = 1300.00;
//static double knivel = 10;
static double knivel = 10;
//static double ktemp = 10;
static double ktemp = 10;
static double Nref=25.00;
static double Tref=15.50;

pthread_t thread21;
pthread_t thread22;
pthread_t thread23;
pthread_t thread24;

static inline double sqrt(double x) {
     double prevresult=1;
     double aux;
     while (1) {
	aux=prevresult;
	aux++;
	if (aux*aux>x) return prevresult;
	prevresult=aux;
     };
};

                        // Temperature Control
void *task21(void *arg) {
       
    pthread_make_periodic_np (pthread_self(), gethrtime(),700000000);
    
    while (1) {
    pthread_wait_np();
    Qt = (Tref - Temp) * ktemp;
    if (Qt<0) Qt=0;
    if (Qt>30.00) Qt=30.00;
    };

}
                         // Nivel Control
void *task22(void *arg) {

    pthread_make_periodic_np (pthread_self(), gethrtime(),700000000);
    while(1) {
    pthread_wait_np();
    Qn = (Nref - Nivel) * knivel;
    if (Qn<0) Qn=0;
    if (Qn>30.00) Qn=30.00;
    };

}
                       // Simulation Task
void *task23(void *arg) {
  double F1,F2;
  double Te=15.00;
  double Tc=45.00;
  double cte1=0.02;
  double cte2=3.0;
  double pvs=0.5;
  int random;
//  double aux;

  // Periódica de medio segundo  
  pthread_make_periodic_np (pthread_self(), gethrtime(),50000000);

  while (1) {
   
    pthread_wait_np();
    rdtscl(random); 
    random=random & 0x1f;
    Qs = (cte2 * sqrt(Nivel)*pvs) + random;

 
    F1 = (Qn * Te) + (Qt * Tc) + ((Qtotal - Qs) * Temp);
    F2 = (Qn + Qt + Qtotal - Qs);
    Temp = F1 / F2;
    if (Temp >= temp_max) Temp = temp_max;
    Nivel = Nivel + ((Qn - Qs + Qt) * cte1);
    if (Nivel >= nivel_max) Nivel = nivel_max;
    Qtotal = Qtotal - Qs + Qn + Qt;
  }
}

#ifndef CONFIG_RTL_GUI

void display_external_tank(void) {
 int x,y;
 y=3;
 rtl_printf("\x1B[0;0m");        // Cambio Color
 rtl_printf("\x1B[1;0H                                                                                ");
 rtl_printf("\x1B[34;47m");        // Cambio Color

 for (x=4;x<21;x++) {
   rtl_printf("\x1B[%d;%dHI",x,y); 
 };
 y=40;
 for (x=4;x<22;x++) {
   rtl_printf("\x1B[%d;%dHI",x,y); 
 };
 x=20;
 for (y=4;y<39;y++) {
   rtl_printf("\x1B[%d;%dH-",x,y); 
 };
 y=38;
 x=21;
 rtl_printf("\x1B[%d;%dHI",x,y); 
 for (y=0;y<4;y++) {
   x=1;
   rtl_printf("\x1B[%d;%dH-",x,y);
   x=3;
   rtl_printf("\x1B[%d;%dH-",x,y);
 };
 for (y=40;y<44;y++) {
   x=1;
   rtl_printf("\x1B[%d;%dH-",x,y);
   x=3;
   rtl_printf("\x1B[%d;%dH-",x,y);
 };
};

void clean_nivel(void) {
 int x;
 rtl_printf("\x1B[0;0m");        // Cambio Color
 for (x=3;x<20;x++) {
  rtl_printf("\x1B[%d;4H                                    ",x);
 };
};

void draw_nivel(int lines) {
 int x;
 int l=lines;
 x=19;
 while (l>0){
  rtl_printf("\x1B[44;44m\x1B[%d;4H                                    ",x);
  l--;
  x--;
 };
// rtl_printf("\x1B[0;0m");        // Cambio Color

 while (x>=3) {
  rtl_printf("\x1B[0;0m\x1B[%d;4H                                    ",x);
  x--;
 };
};

void display_nivel(void) {
 int num_lines;
 num_lines = (int) ((Nivel) * 17.0) / nivel_max;
// clean_nivel();
 draw_nivel(num_lines);
};

void display_variables(void) {
 rtl_printf("\x1B[55;00m");
 rtl_printf("\x1B[3;47H Tank Simulator");
 rtl_printf("\x1B[4;47H --------------");

 rtl_printf("\x1B[6;47H Temp Ref 15.5");
 rtl_printf("\x1B[7;47H Level Ref 25.0");
 
 rtl_printf("\x1B[9;47H Temperature %f %f",Temp,22.3);
 rtl_printf("\x1B[10;47H Level %f %f",Nivel,11.2);
 rtl_printf("\x1B[11;47H Level Control Action: %f",Qn);
 rtl_printf("\x1B[12;47H Temp Control Action: %f",Qt);
 rtl_printf("\x1B[13;47H Out Flow: %f",Qs);
};

void display_tank(void) {
  display_nivel();
  display_variables();
};


#endif // CONFIG_RTL_GUI

#if CONFIG_RTL_GUI
struct picoTerminal term1;
struct picoGC gc;
unsigned char *fbuf;
unsigned int BPP;
#endif

void init_display(void) {
#if CONFIG_RTL_GUI
  fbuf= ((unsigned char *) *((unsigned int *) 0xff0));
  BPP= ((unsigned int) *((char *) 0xff4));
  BPP= BPP/8;
  picoCreateGC(&gc);
  picoSetForeground(&gc,0x1f00ff);
  picoDrawPixmap(&gc,1,1,&pixmap_logo);
  picoSetForeground(&gc,0x00ff00);
  
  picoDrawLine(&gc,100,200,  100,  400);
  picoDrawLine(&gc,300,200,  300,  400);
  picoDrawLine(&gc,100,400,  300,  400);

  picoDrawLine(&gc,100,200,  50,  200);
  picoDrawLine(&gc,300,200,  350, 200 );

  picoDrawLine(&gc,100,180,  50,  180);
  picoDrawLine(&gc,300,180,  350, 180);
  picoSetFont(&gc,&font_8x13);
  picoTerminalCreate(&term1,&gc,400,100,20,7);
#else	
  display_external_tank();
#endif  
};

#if CONFIG_RTL_GUI
void display_graphic_tank(void) {
 int num_lines;
 char text[100];
 num_lines = (int) (((Nivel) * 200.0) / nivel_max);
 
 picoSetForeground(&gc,0x000000);  //Negro
 picoFillRect(&gc,101,200,199,199-num_lines); 
 picoSetForeground(&gc,0x0000ff);  //Azul
 picoFillRect(&gc,101,400-num_lines,199,num_lines); 
 picoSetFont(&gc,&font_8x13);
 
 picoSetBackground(&gc,0x1010ff);  
 picoSetForeground(&gc,0xffffff);  
 
 picoDrawText(&gc,400,50,"Tank Simulator");
 
 picoSetBackground(&gc,0x102f2f);  
 
 
 picoDrawText(&gc,350,80,"Temp Ref: 15.5");
 picoDrawText(&gc,350,100,"Level Ref: 25.0");
 sprintf(text,"Temp : %f ",Temp);
 picoDrawText(&gc,350,120,text);
 sprintf(text,"Level: %f ",Nivel);
 picoDrawText(&gc,350,140,text);
 sprintf(text,"Temp Control Action: %f",Qt);
 picoDrawText(&gc,350,160,text);
 sprintf(text,"Level Control Action: %f",Qn);
 picoDrawText(&gc,350,180,text);
 sprintf(text,"Out Flow: %f",Qs);
};
#endif

void *task24(void *arg)
{
  int aux=1;	
  init_display();
  pthread_make_periodic_np (pthread_self(), gethrtime(),200000000);
  while (1) {
    pthread_wait_np();
    aux++;
    *(fbuf+aux)=0x5a;

#if CONFIG_RTL_GUI
    display_graphic_tank();
#else    
    display_tank();
#endif    
  };
};

#endif // TANK_EXAMPLE_NO_FB