#include "SPI.h"
#include <Wire.h>
#include "Adafruit_GFX.h"
#include "Adafruit_ILI9341.h"
#include "PinChangeInterrupt.h"
#include "AS5600.h"
#include "EEPROM.h"
//#include <Fonts/FreeMonoBoldOblique12pt7b.h>
#include <Fonts/FreeMonoBoldOblique18pt7b.h>


/*
FreeMono12pt7b.h		FreeSansBoldOblique12pt7b.h
FreeMono18pt7b.h		FreeSansBoldOblique18pt7b.h
FreeMono24pt7b.h		FreeSansBoldOblique24pt7b.h
FreeMono9pt7b.h			FreeSansBoldOblique9pt7b.h
FreeMonoBold12pt7b.h		FreeSansOblique12pt7b.h
FreeMonoBold18pt7b.h		FreeSansOblique18pt7b.h
FreeMonoBold24pt7b.h		FreeSansOblique24pt7b.h
FreeMonoBold9pt7b.h		FreeSansOblique9pt7b.h
FreeMonoBoldOblique12pt7b.h	FreeSerif12pt7b.h
FreeMonoBoldOblique18pt7b.h	FreeSerif18pt7b.h
FreeMonoBoldOblique24pt7b.h	FreeSerif24pt7b.h
FreeMonoBoldOblique9pt7b.h	FreeSerif9pt7b.h
FreeMonoOblique12pt7b.h		FreeSerifBold12pt7b.h
FreeMonoOblique18pt7b.h		FreeSerifBold18pt7b.h
FreeMonoOblique24pt7b.h		FreeSerifBold24pt7b.h
FreeMonoOblique9pt7b.h		FreeSerifBold9pt7b.h
FreeSans12pt7b.h		FreeSerifBoldItalic12pt7b.h
FreeSans18pt7b.h		FreeSerifBoldItalic18pt7b.h
FreeSans24pt7b.h		FreeSerifBoldItalic24pt7b.h
FreeSans9pt7b.h			FreeSerifBoldItalic9pt7b.h
FreeSansBold12pt7b.h		FreeSerifItalic12pt7b.h
FreeSansBold18pt7b.h		FreeSerifItalic18pt7b.h
FreeSansBold24pt7b.h		FreeSerifItalic24pt7b.h
FreeSansBold9pt7b.h		FreeSerifItalic9pt7b.h

*/





#define BLACK   0x0000
#define BLUE    0xF300 //0xF800
#define RED     0x001F
#define GREEN   0x03E0 //0xF81F
#define DGREEN  0x0300 //0xF81F
#define CYAN    0xFF70 //0xFFE0
#define MAGENTA 0xF18F //0x07E0
#define YELLOW  0x0EFF //0x07FF
#define WHITE   0xFFFF

// pin definition for the Uno

#define rst  A0
#define TFT_DC 8
#define TFT_CS 12

#define AS5600_1_SCL  A5
#define AS5600_1_SDA  A4

#define EncButtonPin 2
#define Encoder2Pin 5
#define Encoder1Pin 4

#define PWMOut 3
#define EndSw 6

#define DirectionPin A7

#define EndSwitchPin A6

#define DRV1_DIR 10
#define DRV1_STEP 9
#define DRV_SLEEP 7

#define DRV2_DIR  A1
#define DRV2_STEP A2


#define AS5600_2_an A6
#define AS5600_SW A3


#ifdef ARDUINO_SAMD_VARIANT_COMPLIANCE
  #define SERIAL SerialUSB
  #define SYS_VOL   3.3
#else
  #define SERIAL Serial
  #define SYS_VOL   5
#endif

Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);


uint8_t changeButton = 0;

//int8_t motorPH = 0;
bool setButton = false;

struct Motor
{
  int32_t position;  
  int  fordulat;
  bool dir;
  bool sleep;
};

struct EncoderData
{
  int8_t oldPos;
  uint32_t encoderPosition;
  bool encSignalChanged;
};



Motor m1;
Motor m2;
EncoderData encoder_Data;
//EncoderData encoder_Data;
AS5600 as5600;   //  use default Wire -> SDA= A4 ; SCL=A5
void ISRButton()
{  
  if(!digitalRead(EncButtonPin))
  {
    changeButton++;
    if(changeButton > 3)
    changeButton = 0;
   
    setButton = true;
    encoder_Data.encoderPosition = 512;
    
  }
  
}

int8_t encPosition()
{
  int8_t encPos = digitalRead(Encoder1Pin);  
  encPos = (encPos<<1)|digitalRead(Encoder2Pin);    
  return encPos;
}
void ISREncoderSignal()
{ 
    int position  = 0;
    int8_t encPos = encPosition();
    switch(encoder_Data.oldPos)
    {
      case 0:
      {
        if(encPos == 1)
          position--;
        else  
        if(encPos == 2)
          position++;  
        break;
      }
      case 1:
      {
        if(encPos == 3)
          position--;
        else  
        if(encPos == 0)
          position++;  
        break;
      }
      case 2:
      {
        if(encPos == 0)
          position--;
        else  
        if(encPos == 3)
          position++;  
        break;
      }
      case 3:
      {
        if(encPos == 2)
          position--;
        else  
        if(encPos == 1)
          position++;  

        break;
      }
    }
   
  encoder_Data.oldPos = encPos;
  encoder_Data.encSignalChanged = true;
  encoder_Data.encoderPosition+=position; 
  if( encoder_Data.encoderPosition <1)
   encoder_Data.encoderPosition =1;
   
}

void Motor1Handler(int position, bool dir )
{
  if(!m1.sleep)
  {
  
  }
    

}

void ISREndPozicio()
{
  analogRead(EndSwitchPin);


}

void MagMagnitude(uint16_t readMag)
{
  
  switch(readMag)
  {
    case 0:
    {
      
      tft.setTextColor(ILI9341_RED);
      tft.println("no magnet!");
      break;
    } 
    case 1:
    {
      
      tft.setTextColor(ILI9341_ORANGE);
      tft.println("weak magnet");
      break;
    } 
    case 2:
    {
     
      tft.setTextColor(ILI9341_GREEN);
      tft.println("good magnet");
      break;
    } 
    case 3:
    {      
      tft.setTextColor(ILI9341_GREEN);
      tft.println("strong magnet");
      break;
    } 
  }

}

void setup() 
{
  attachInterrupt(digitalPinToInterrupt(EncButtonPin), ISRButton, CHANGE);
  attachPCINT(digitalPinToPCINT(Encoder1Pin), ISREncoderSignal,CHANGE );
  attachPCINT(digitalPinToPCINT(Encoder2Pin), ISREncoderSignal,CHANGE );
 // attachPCINT(digitalPinToPCINT(EndSwitchPin), ISREndPozicio,CHANGE );



    
  pinMode(DRV1_DIR, OUTPUT);
  pinMode(DRV1_STEP, OUTPUT);
  pinMode(DRV_SLEEP, OUTPUT);
   pinMode(DRV2_DIR, OUTPUT);
  pinMode(DRV2_STEP, OUTPUT);
  pinMode(AS5600_SW, OUTPUT);

   Wire.begin();                                         // start i2C
    Wire.setClock(400000L);                               // fast clock
    as5600.begin(DirectionPin);  //  set direction pin.
    as5600.setDirection(AS5600_CLOCK_WISE);  // default, just be explicit.


  tft.begin();
  tft.setFont(&FreeMonoBoldOblique18pt7b);
  tft.setRotation(3);
  tft.setCursor(5, 100);
  tft.setTextColor(ILI9341_GREEN);
  tft.setTextSize(1);
  digitalWrite(AS5600_SW, HIGH);
  tft.println("Winding machine");


 tft.setCursor(1, 130);
  tft.setTextColor(ILI9341_WHITE);  
  tft.print("M0:");
  MagMagnitude(as5600.readMagnitude());

  tft.setCursor(1, 160);
  tft.setTextColor(ILI9341_WHITE);  
  tft.print("M1:");
  MagMagnitude(as5600.readMagnitude()); 
/*
   switch(as5600.readMagnitude())
   {
    case 0:
    {
      tft.setCursor(1, 130);
      tft.setTextColor(ILI9341_RED);
      tft.println("M1:no magnet!");
      break;
    } 
    case 1:
    {
      tft.setCursor(1, 130);
      tft.setTextColor(ILI9341_ORANGE);
      tft.println("M1:weak magnet");
      break;
    } 
    case 2:
    {
      tft.setCursor(1, 130);
      tft.setTextColor(ILI9341_GREEN);
      tft.println("M1:good magnet");
      break;
    } 
    case 3:
    {
      tft.setCursor(1, 130);
      tft.setTextColor(ILI9341_GREEN);
      tft.println("M1:strong magnet");
      break;
    } 
*/
 delay(500);
  }
   




 


void loop() 
{
  
 
  analogRead(AS5600_2_an);
  

}