#include "Ananas.h"

#include "ConfigurationStore.h"

#include "stepper.h"
#include "Encoder.h"
#include "configuration.h"
#include "con_configuration.h"
#include "controller.h"
#include "language.h"
#include "PWMDA.h"



//EEPROM Read And Write
void _EEPROM_writeData(int &pos, uint8_t* value, uint8_t size) {
	do {
		eeprom_write_byte((unsigned char*) pos, *value);
		pos++;
		value++;
	} while (--size);
}
#define EEPROM_WRITE_VAR(pos, value) _EEPROM_writeData(pos, (uint8_t*)&value, sizeof(value))

void _EEPROM_readData(int &pos, uint8_t* value, uint8_t size) {
	do {
		*value = eeprom_read_byte((unsigned char*) pos);
		pos++;
		value++;
	} while (--size);
}
#define EEPROM_READ_VAR(pos, value) _EEPROM_readData(pos, (uint8_t*)&value, sizeof(value))
//======================================================================================

#define EEPROM_OFFSET 100

// IMPORTANT:  Whenever there are changes made to the variables stored in EEPROM
// in the functions below, also increment the version number. This makes sure that
// the default values are used whenever there is a change to the data, to prevent
// wrong data being written to the variables.
// ALSO:  always make sure the variables in the Store and retrieve sections are in the same order.
#define EEPROM_VERSION "V10"

#ifdef EEPROM_SETTINGS
void Config_StoreSettings() {
	char ver[4] = "000";
	int i = EEPROM_OFFSET;
	EEPROM_WRITE_VAR(i, ver); // invalidate data first

	//PID
	EEPROM_WRITE_VAR(i, kp);
	EEPROM_WRITE_VAR(i, ki);
	EEPROM_WRITE_VAR(i, kd);
	//parameter PID need
	EEPROM_WRITE_VAR(i, start_rate);
	EEPROM_WRITE_VAR(i, jerk_rate);
	EEPROM_WRITE_VAR(i, max_feedrate);
	EEPROM_WRITE_VAR(i, acceleration_rate);

	EEPROM_WRITE_VAR(i, controldir);
	EEPROM_WRITE_VAR(i, inverseCountDir);

	EEPROM_WRITE_VAR(i, factor);

	EEPROM_WRITE_VAR(i, subdivide);

	EEPROM_WRITE_VAR(i, speedfactor);
	EEPROM_WRITE_VAR(i, stepcountdir);

	char ver2[4] = EEPROM_VERSION;
	i = EEPROM_OFFSET;
	EEPROM_WRITE_VAR(i, ver2); // validate data
	SERIAL_ECHO_START;
	SERIAL_ECHOLNPGM("Settings Stored");
}
#endif //EEPROM_SETTINGS

#ifndef DISABLE_M503
void Config_PrintSettings() { // Always have this function, even with EEPROM_SETTINGS disabled, the current values will be shown

	SERIAL_ECHO_START;

	SERIAL_ECHOPGM("Machine Name : ");
	SERIAL_ECHOLNPGM(MACHINE_NAME);
//	SERIAL_ECHOPGM(MSG_AUTHOR);
//	SERIAL_ECHOLNPGM(" Dark Guan");
	SERIAL_ECHOPGM("URL: ");
	SERIAL_ECHOLNPGM(FIRMWARE_URL);
	SERIAL_ECHOPGM("Version: ");
	SERIAL_ECHOLNPGM(PROTOCOL_VERSION);
//	SERIAL_ECHOLN("");
	SERIAL_ECHOLNPGM("M51  Change SpeedFactor!");
	SERIAL_ECHOPAIR("M51 S", (unsigned long )speedfactor);
	SERIAL_ECHOLN("");
	SERIAL_ECHOLNPGM("M56  Change Encoder Subdivide ,Reset Needed!");
	SERIAL_ECHOLNPGM("1 2 and 4 is allowed only");
	SERIAL_ECHOPAIR("M56 S", (unsigned long )subdivide);
	SERIAL_ECHOLN("");

	SERIAL_ECHOLNPGM("M60  Set PID Parameters !");
	SERIAL_ECHOPAIR("M60 P", kp);
	SERIAL_ECHOPAIR(" I", ki);
	SERIAL_ECHOPAIR(" D", kd);
	SERIAL_ECHOLN("");
//	SERIAL_ECHO_START;
	SERIAL_ECHOPAIR("M65 S", (unsigned long )start_rate);
	SERIAL_ECHOPAIR(" J", (unsigned long )jerk_rate);
	SERIAL_ECHOPAIR(" X", (unsigned long )max_feedrate);
	SERIAL_ECHOPAIR(" A", acceleration_rate);
	SERIAL_ECHOLN("");
	SERIAL_ECHOPAIR("M66 S", factor);
	SERIAL_ECHOLN("");
	SERIAL_ECHOPAIR("The EncoderCountDIR is ", inverseCountDir);
	SERIAL_ECHOLN("");
	SERIAL_ECHOLNPGM("M67 can change the Encoder Count direction.");
//	SERIAL_ECHOLN("");
	SERIAL_ECHOPAIR("The StepDir is ", controldir);
	SERIAL_ECHOLN("");
	SERIAL_ECHOLNPGM("M68 can change  Stepper  direction.");
//	SERIAL_ECHOLN("");

	SERIAL_ECHOPAIR("The StepCountDIR is ", stepcountdir);
	SERIAL_ECHOLN("");
	SERIAL_ECHOLNPGM("M69 can change Steps counts  direction.");
	SERIAL_ECHOLN("");
}
#endif

#ifdef EEPROM_SETTINGS
void Config_RetrieveSettings() {
	int i = EEPROM_OFFSET;
	char stored_ver[4];
	char ver[4] = EEPROM_VERSION;
	EEPROM_READ_VAR(i, stored_ver); //read stored version

//	  SERIAL_ECHOLN("Version: " );
//	  SERIAL_ECHOLN(ver);
//	  SERIAL_ECHOLN("Stored version: ");
//	  SERIAL_ECHOLN(stored_ver);

	if (strncmp(ver, stored_ver, 3) == 0) {

		//PID
		EEPROM_READ_VAR(i, kp);
		EEPROM_READ_VAR(i, ki);
		EEPROM_READ_VAR(i, kd);

		setPID(kp, ki, kd);

		//parameter PID need
		EEPROM_READ_VAR(i, start_rate);
		setstartfeedrate(start_rate);

		EEPROM_READ_VAR(i, jerk_rate);
		setjerkfeedrate(jerk_rate);

		EEPROM_READ_VAR(i, max_feedrate);
		refreshmaxfeedrate();
		EEPROM_READ_VAR(i, acceleration_rate);

		EEPROM_READ_VAR(i, controldir);
		refreshPID();

		EEPROM_READ_VAR(i, inverseCountDir);


		setincrement();

		EEPROM_READ_VAR(i, factor);
		EEPROM_READ_VAR(i, subdivide);
		EEPROM_READ_VAR(i, speedfactor);
		refresh_endertostep();

		EEPROM_READ_VAR(i, stepcountdir);

		SERIAL_ECHO_START;
		SERIAL_ECHOLNPGM("Stored settings retrieved");
	} else {
		Config_ResetDefault();
	}
#ifdef EEPROM_CHITCHAT //  print
	Config_PrintSettings();
#endif
}
#endif //

void Config_ResetDefault() {

	//PID
	kp = KP;
	ki = KI;
	kd = KD;

	setPID(kp, ki, kd);
	//parameter PID need

	max_feedrate = USR_MAX_FEEDRATE;
	refreshmaxfeedrate();

	acceleration_rate = DEFAULT_ACC_RATE;

	setjerkfeedrate(JERK_FEEDRATE);
	setstartfeedrate(START_FEEDRATE);

	controldir = false;
	inverseCountDir = INV_COUNT_DIR;
	setincrement();

	factor = STRONG_PURSE_FACTOR;
	encoderfactor = 1;

	speedfactor = STRONG_PURSE_FACTOR;
	subdivide = ENCODER_SUBDIVIDE;

	endertostep = SPEEDUNIT * STRONG_PURSE_FACTOR;

	stepcountdir = false;


	SERIAL_ECHO_START;
	SERIAL_ECHOLNPGM("Hardcoded Default Settings Loaded");

}