#include "modelec.h"

#include <algorithm>
#include <cmath>

bool DiffBot::isDelayPassedFrom(uint32_t delay, uint32_t &lastTick) {
	if (HAL_GetTick() - lastTick >= delay) {
		lastTick = HAL_GetTick();
		return true;
	}
	return false;
}

bool DiffBot::isDelayPassed(uint32_t delay) {
	return isDelayPassedFrom(delay, lastTick);
}

float DiffBot::readEncoderLeft() {
	int16_t count = __HAL_TIM_GET_COUNTER(&htim2);
	int16_t diff = count - prevCountLeft;
	prevCountLeft = count;
	float revs = static_cast<float>(diff) / ENCODER_RES;
	return (2.0f*M_PI*WHEEL_RADIUS*revs); // m
}

float DiffBot::readEncoderRight() {
    int16_t count = __HAL_TIM_GET_COUNTER(&htim3);
    int16_t diff = count - prevCountRight;
    prevCountRight = count;
    float revs = static_cast<float>(diff) / ENCODER_RES;
    return (2.0f*M_PI*WHEEL_RADIUS*revs); // m
}

DiffBot::DiffBot(Point pos, float dt) : pos(pos), dt(dt) {
};

void DiffBot::stop(bool stop) {
	odo_active = !stop;
	motor.stop(stop);
}

void DiffBot::setup() {
	pidLeft = PID(1, 0.0, 0.0, -PWM_MAX, PWM_MAX);
	pidRight = PID(1, 0.0, 0.0, -PWM_MAX, PWM_MAX);
	pidPos = PID(1, 0.0, 0.0, -V_MAX, V_MAX);
	pidTheta = PID(2, 0.0, 0.0, -M_PI, M_PI);

	prevCountLeft = __HAL_TIM_GET_COUNTER(&htim2);
	prevCountRight = __HAL_TIM_GET_COUNTER(&htim3);
}

void DiffBot::update(float dt) {
	if (!isDelayPassed(dt*1000)) return;

	// read encoder
    float rightVel = readEncoderRight();
    float leftVel = readEncoderLeft();

    if (rightVel == 0 && leftVel == 0) {
    	if (motor.rightTarget_PWM != 0 || motor.leftTarget_PWM != 0) {
        	if (!no_move) {
        		no_move = true;
        		publishNotMoved = HAL_GetTick();
        	}
        	else if (isDelayPassedFrom(notMovedMaxTime, publishNotMoved)) {
        		motor.stop(true);

        		targets[index].active = false;

    			no_move = false;

        		if (action != 0) {
        			switch (action) {
        			case 1: // LEFT
        				if (cos(pos.theta) > 0) {
        					pos.x = 0.1f; // dist fron back to center
        				}
        				else {
        					pos.x = 0.1f; // dist fron center to front
        				}
        				break;
        			case 2: // TOP
        				if (sin(pos.theta) > 0) {
        					pos.y = 2.0f - 0.1f; // dist fron center to front
        				}
        				else {
        					pos.y = 2.0f - 0.1f; // dist fron back to center
        				}
        				break;
        			case 3: // RIGHT
        				if (cos(pos.theta) > 0) {
        					pos.x = 3.0f - 0.1f; // dist fron center to front
        				}
        				else {
        					pos.x = 3.0f - 0.1f; // dist fron back to center
        				}
        				break;
        			case 4: // BOTTOM
        				if (sin(pos.theta) > 0) {
        					pos.y = 0.1f; // dist fron back to center
        				}
        				else {
        					pos.y = 0.1f; // dist fron center to front
        				}
        				break;
        			default:
        				break;
        			}

        			action = 0;

        			addTarget(0, 1, pos.x, pos.y, pos.theta);
        		}
        	}
    	}
    } else {
    	if (no_move) {
    		no_move = false;
    	}
    }

    // update pos
    float v = (leftVel + rightVel) / 2.0f;
    float w = (rightVel - leftVel) / WHEEL_BASE;
    pos.x += v * cosf(pos.theta - w/2);
    pos.y += v * sinf(pos.theta - w/2);
    pos.theta += w;

    while (pos.theta >  M_PI) pos.theta -= 2*M_PI;
    while (pos.theta < -M_PI) pos.theta += 2*M_PI;

    if (odo_active && isDelayPassedFrom(frequencyPublish, publishLastTick)) {
        publishStatus();
    }

    if (!odo_active || !targets[index].active) {
    	motor.update();
    	return;
    }

    // pid setup
    float dx = targets[index].x - pos.x;
    float dy = targets[index].y - pos.y;

    switch (targets[index].state) {
    case FINAL:

    	if (std::fabs(dx) <= precisePosFinal && std::fabs(dy) <= precisePosFinal && std::fabs(targets[index].theta - pos.theta) < preciseAngle) {
    		targets[index].active = false;
    		motor.stop(true);

    		char log[64];
    		sprintf(log, "SET;WAYPOINT;REACH;%d\n", index);
    		CDC_Transmit_FS((uint8_t*)log, strlen(log));

    		resetPID();

    		return;
    	}

    	break;
    case INTERMEDIAIRE:

    	if (std::fabs(dx) < precisePos && std::fabs(dy) < precisePos) {

    		char log[64];
    		sprintf(log, "SET;WAYPOINT;REACH;%d\n", index);
    		CDC_Transmit_FS((uint8_t*)log, strlen(log));

    		targets[index].active = false;
    		index++;

    		if (index >= 9) {
    			index = 0;
    			return;
    		}

    		resetPID();

    		dx = targets[index].x - pos.x;
    		dy = targets[index].y - pos.y;

    	}

    	break;
    default:
    	break;
    }

	float dist = sqrtf(dx*dx + dy*dy);

	float angleTarget = atan2f(dy, dx);
	float angleError = angleTarget - pos.theta;

	while (angleError > M_PI) angleError -= 2*M_PI;
	while (angleError < -M_PI) angleError += 2*M_PI;

	float direction = 1.0f;
	if (std::fabs(angleError) > M_PI/2) {
		direction = -1.0f;
		angleError > 0 ? angleError -= M_PI : angleError += M_PI;
	}

	if (std::fabs(angleError) <= precisePosFinal) angleError = 0;

	float distError = dist * cosf(angleError);

	distError *= direction;

    float vRef = pidPos.compute(0.0, -distError, dt);
	float wRef;

    if (targets[index].state == FINAL && std::fabs(dx) <= precisePosFinal && std::fabs(dy) <= precisePosFinal) {
        wRef = pidTheta.compute(targets[index].theta, pos.theta, dt);
        vRef = 0;
    }
    else {
        wRef = pidTheta.compute(0.0, -angleError, dt);
    }

    float vLeft = vRef - (WHEEL_BASE_2) * wRef;
    float vRight = vRef + (WHEEL_BASE_2) * wRef;

    float pwm_ff_left = (vLeft / V_MAX) * PWM_MAX;
    float pwm_ff_right = (vRight / V_MAX) * PWM_MAX;

    float pwm_corr_left = pidLeft.compute(vLeft, leftVel, dt);
    float pwm_corr_right = pidRight.compute(vRight, rightVel, dt);

    float pwmLeft = pwm_ff_left + pwm_corr_left;
    float pwmRight = pwm_ff_right + pwm_corr_right;

    const float pwm_deadzone = 50.0f;
    if (std::fabs(pwmLeft) > 0 && std::fabs(pwmLeft) < pwm_deadzone)
        pwmLeft = (pwmLeft > 0) ? pwm_deadzone : -pwm_deadzone;
    if (std::fabs(pwmRight) > 0 && std::fabs(pwmRight) < pwm_deadzone)
        pwmRight = (pwmRight > 0) ? pwm_deadzone : -pwm_deadzone;

    // saturation
    pwmLeft  = std::max(-PWM_MAX, std::min(pwmLeft,  PWM_MAX));
    pwmRight = std::max(-PWM_MAX, std::min(pwmRight, PWM_MAX));

    motor.leftTarget_PWM = static_cast<int16_t>(pwmLeft);
    motor.rightTarget_PWM = static_cast<int16_t>(pwmRight);
    motor.update();
}

void DiffBot::addTarget(int id, int type, float x, float y, float theta) {

	if (id >= MAX_WAYPOINTS) return;

	targets[id] = Point(id, static_cast<StatePoint>(type), x, y, theta);
	targets[id].active = true;

	if (id <= index) index = 0;

    arrive = false;
}

void DiffBot::resetPID() {
	pidLeft.reset();
	pidRight.reset();
	pidPos.reset();
	pidTheta.reset();
}

void DiffBot::publishStatus() {
    char response[64];
    snprintf(response, sizeof(response), "SET;POS;%.4f;%.4f;%.4f\n", pos.x*1000, pos.y*1000, pos.theta);
    CDC_Transmit_FS((uint8_t*)response, strlen(response));
}