/*
 * AS angle
 * Module à placer sur le panneau solaire
 * Le module mesure 
 *  1.l'inclinaison sur l'axe Y (à vérifier)
 *  2.l'orientation de 0 à 360°
 *  Les mesures snt envoyés vers rapidSCADA en modbus
 *  Une page web embarqué permet de voir les mesures en direct //option
 *  Un affichage LCD affiche : les mesures et l'adresse IP.
 *  La configuration se fera par reprogrammation ou/et par la page web
 * 
 * 
 * 
 * 
 * 
 */
#include <M5StickCPlus.h>
#include <Wire.h>
#include <LSM303.h>
#include <WiFi.h>
#include <ModbusIP_ESP8266.h>

LSM303 compass;

//modbus
const int Azimut_HREG = 200;
const int Elevation_HREG=201;

//ModbusIP object
ModbusIP mb;
IPAddress staticIP(192, 168, 0, 60); 
IPAddress subnet(255, 255, 255, 0);
IPAddress gateway(192, 168, 0, 254);

long ts;

/*******************************************/
/******************************************/

void setup() {
  Serial.begin(115200);

  // initialize the M5StickC object
  M5.begin();
  M5.Lcd.setRotation(3);
  // text print
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setCursor(0, 10);
  M5.Lcd.setTextColor(WHITE);
  M5.Lcd.setTextSize(2);
  M5.Lcd.printf("ASangle v2");
//

//WIFI 
  M5.Lcd.setCursor(0,110 );
  M5.Lcd.printf("Attente Wifi...");
  if (WiFi.config(staticIP, gateway, subnet) == false) {
    Serial.println("Configuration failed.");
  }

    WiFi.begin("default", "");
    
    while (WiFi.status() != WL_CONNECTED) {
        delay(500);
        Serial.print(".");
    }

    Serial.println("");
    Serial.println("WiFi connected");  
    Serial.println("IP address: ");
    Serial.println(WiFi.localIP());
    M5.Lcd.fillRect(0,110,240,25,BLACK);
    M5.Lcd.setCursor(0,110);M5.Lcd.printf("Wifi ok  ");



//finWIFI


//Modbus
    
    mb.server();    //Start Modbus IP
    // Add SENSOR_IREG register - Use addHreg()
    mb.addHreg(Azimut_HREG);
    mb.addHreg(Elevation_HREG);

//fin Modbus

//compass
  Wire.setPins(26,0);//attribut les broche I2C (bool setPins(int sdaPin, int sclPin);)
  Wire.begin();//changer les port I2C.
  compass.init();
  compass.enableDefault();
  
  /*
  Calibration values; the default values of +/-32767 for each axis
  lead to an assumed magnetometer bias of 0. Use the Calibrate example
  program to determine appropriate values for your particular unit.
  */
  compass.m_min = (LSM303::vector<int16_t>){-32767, -32767, -32767};
  compass.m_max = (LSM303::vector<int16_t>){+32767, +32767, +32767};
//fin compass
  ts = millis();
}//fin setup


/*************************************************/
/*************************************************/
void loop() {

int valAzimut, valElevation;
  //compass
  compass.read();
  
  /*
  When given no arguments, the heading() function returns the angular
  difference in the horizontal plane between a default vector and
  north, in degrees.
  
  The default vector is chosen by the library to point along the
  surface of the PCB, in the direction of the top of the text on the
  silkscreen. This is the +X axis on the Pololu LSM303D carrier and
  the -Y axis on the Pololu LSM303DLHC, LSM303DLM, and LSM303DLH
  carriers.
  
  To use a different vector as a reference, use the version of heading()
  that takes a vector argument; for example, use
  
    compass.heading((LSM303::vector<int>){0, 0, 1});
  
  to use the +Z axis as a reference.
  */
  float heading = compass.heading();
  int angleX = (int)compass.a.x;
  int angleY = (int)compass.a.y;
  int angleZ = (int)compass.a.z;

  int16_t inclination = atan2(angleY, sqrt(angleX * angleX + angleZ * angleZ)) * 180.0 /PI;
  valAzimut = round(heading) ;
  valElevation = inclination;
  Serial.println(heading);
  Serial.println(valElevation);
  
  //affichage vers LCD
IPAddress ipAdr;
  ipAdr = WiFi.localIP();
  M5.Lcd.fillRect(0,110,240,25,BLACK);
  M5.Lcd.setCursor(0,110);  M5.Lcd.setTextSize(2);  M5.Lcd.setTextColor(BLUE);
  M5.Lcd.printf("IP:%u.%u.%u.%u\n\r",ipAdr[0],ipAdr[1],ipAdr[2],ipAdr[3]);  
  
  M5.Lcd.setCursor(10, 40);
  M5.Lcd.setTextColor(RED);
  M5.Lcd.setTextSize(3);
  M5.Lcd.printf("Bous:%6d",(int)heading);
  M5.Lcd.setCursor(10, 80);
  M5.Lcd.setTextColor(RED);
  M5.Lcd.setTextSize(3);
  M5.Lcd.printf("Angle:%6d",(int)inclination);  
  delay(1000);
  M5.Lcd.fillRect(140,39,90,25,WHITE);//M5.Lcd.fillRect(int16_t x, int16_t y, int16_t w, int16_t h, uint16_t color);
  M5.Lcd.fillRect(140,79,90,25,WHITE);//M5.Lcd.fillRect(int16_t x, int16_t y, int16_t w, int16_t h, uint16_t color);

//modbus
   //ModBus Call once inside loop() - all magic here
   mb.task();
   //Read each two seconds
   if (millis() > ts + 2000) {
       ts = millis();
       mb.Hreg(Azimut_HREG,valAzimut);
       mb.Hreg(Elevation_HREG,valElevation);
   
       Serial.println("envoi modbus");
   }
   delay(10);
//fin modbus
  
}