#include "Top_Aerogenator.c"
#include <SPI.h>           // Not needed with I2C
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Servo.h>
Servo servoMotorB1;
Servo servoMotorB2;
Servo servoMotorB3;
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET     4 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

void show() { // Often used sequence - Function to simplify code
  display.display(); delay(2000); display.fillScreen(SSD1306_BLACK);
}

#define velocity_wind_input A0
#define analog_input_B1 A1
#define analog_input_B2 A2
#define analog_input_B3 A3



#define analog_output_B1 11
#define analog_output_B2 10
#define analog_output_B3 9

#define LED_OK 4
#define LED_WARNING 3
#define LED_DANGEROUS 2

outC_Top_Aerogenator outC;
inC_Top_Aerogenator inC;
void setup() {
   Serial.begin(9600);
   if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3D)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }
  // put your setup code here, to run once:
    pinMode(velocity_wind_input, INPUT); 
    pinMode(analog_input_B1, INPUT); 
    pinMode(analog_input_B2, INPUT); 
    pinMode(analog_input_B3, INPUT); 
    pinMode(analog_output_B1, OUTPUT);
    pinMode(analog_output_B2, OUTPUT);
    pinMode(analog_output_B3, OUTPUT);
    pinMode(LED_OK, OUTPUT);
    pinMode(LED_WARNING, OUTPUT);
    pinMode(LED_DANGEROUS, OUTPUT);
   void Top_Aerogenator_init(outC_Top_Aerogenator *outC);
    // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Old Address 0x3D for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }
  
// Initialise variables
  int x0 = 5; // 3 pairs of co-ordinates
  int y0 = 5;
  int x1 = 120;
  int y1 = 23;
  int x2 = 55;
  int y2 = 60;
  int w = 105; // width
  int h = 55;  // height
  int r = 25;  // radius
  int cw = SSD1306_WHITE; // colour white
  int cb = SSD1306_BLACK; // colour black
  int wait = 2000; // 2 second delay
    ///servomotores
  servoMotorB1.attach(9);
  servoMotorB2.attach(10);
  servoMotorB3.attach(11);


  


}

void loop() {
  // de 0 a 50 los
  //sumar 25 al angulo calculado por la curva.
  
  inC.velocity_wind = map(analogRead(velocity_wind_input), 0, 1023, 0, 30); // 0 a 30m/s
  inC.velocity_blade = 60.0;
  inC.run= true;
  inC.type_= true;
  //inC.real_angleB1 = map(analogRead(analog_input_B1), 0, 1023, 0, 33);
  inC.real_angleB1 = map(analogRead(analog_input_B1), 0, 1023, 0, 50);
  inC.real_angleB2 = map(analogRead(analog_input_B2), 0, 1023, 0, 50);
  inC.real_angleB3 = map(analogRead(analog_input_B3), 0, 1023, 0, 50);
  inC.type_ =  true;

  // put your main code here, to run repeatedly:
  Top_Aerogenator(&inC, &outC);
  //Serial.println(outC.Pm);
  //Serial.println("SetPoint");
  //Serial.println(outC.angle);

  //Serial.println("Real angle");
  //Serial.println(inC.real_angleB1);
  
  //println(outC.statusSystemOperation);
  //println(outC.statusSystem);
  Serial.println("Salida analogica Blade 1");
  Serial.println(outC.Blade1);

  Serial.println("Salida analogica Blade 2");
  Serial.println(outC.Blade2);

  Serial.println("Salida analogica Blade 3");
  Serial.println(outC.Blade3);

  //println(outC.Blade2);
  //println(outC.Blade3);
  if (outC.statusSystemOperation == false && outC.statusSystem == false)
  {
    digitalWrite(LED_OK, true);
  }else{
    digitalWrite(LED_OK, false);
  }

// Title screen
  display.clearDisplay();
  display.setTextSize(1);             // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(10,0);
  display.println("Analogicas:");
  display.setCursor(10,10);
  display.println(outC.Blade1);
  servoMotorB1.write(outC.Blade1);
  display.setCursor(50,10);
  servoMotorB2.write(outC.Blade2);
  display.println(outC.Blade2);
  display.setCursor(90,10);
  servoMotorB3.write(outC.Blade3);
  display.println(outC.Blade3);
  display.setCursor(10,20);
  display.println("Wind(m/s):");
  display.setCursor(10,30);
  display.println(inC.velocity_wind);

  display.setCursor(10,40);
  display.println("PM (KwH):");
  display.setCursor(10,50);
  display.println(outC.Pm/1000);

  display.setCursor(70,20);
  display.println("Angle (d):");
  display.setCursor(70,30);
  display.println(outC.angle);
  //display.display(); delay(2000);
  randomSeed(analogRead(3));
  display.display();

  digitalWrite(LED_WARNING,outC.statusSystemOperation);
  digitalWrite(LED_DANGEROUS,outC.statusSystem);
}